


f 


j! S nN 
iby 4 ‘ ts Me , 









Institutional Archive of the Naval Postgraduate School 


Calhoun: The NPS Institutional Archive 
DSpace Repository 


Theses and Dissertations 1. Thesis and Dissertation Collection, all items 


1988 
Local path planning using optimal control techniques 
Smith, Winston. 

Monterey, California. Naval Postgraduate School 


http://ndl.handle.net/10945/23065 


Downloaded from NPS Archive: Calhoun 


| Calhoun is the Naval Postgraduate School's public access digital repository for 
F (8 D U DLEY research materials and institutional publications created by the NPS community. 
«ist | | Calhoun is named for Professor of Mathematics Guy K. Calhoun, NPS'‘s first 


lil \ KNOX appointed -— and published -—- scholarly author. 


http://www.nps.edu/library 






LIBRARY Dudley Knox Library / Naval Postgraduate School 
411 Dyer Road / 1 University Circle 
Monterey, California USA 93943 


Od 


ee et ah ee 


=} 





a a ar ns Au ro miagaret ate weie.t ssh 


; ok ; oN fe . r 
} ae "4 : 7 eat t, Ute fred ‘qh cage? Eaton eon Peer 7 
z : ‘ ire ut pepo: totshs, 















— Fide ephs °& Le MES s. 
4.0 Pr ae eveye | RT | 

as . ij , + 9. AER AK WR 4 YALEL Te 2. RESO A EAtRg OdAS Seb cA ae 
‘ ' oa . . 1 a » ust > 2 at ye 0 berate Chey eae. &, bade war RArte® 
%, ” Meee : a me 5 ' ye oy: Da Beh hear! 
. ' . 2A tf ‘a. tes Wea: weheid » € ware WIS - ae) eels OFA MEee, O1 OM 

« Mh tee "ETAL, Agi atltte sbiade? P = mr te BRhs AATAT Magi hm fdrpalagnos aropeges Pie were tt a) 
. aa ' 44 asl A i nn ee ryt tha 8 BPai@e or fa, Wa Bits DORPAGIT Re ee CA 
ue ? th PUES oP Shih bi vel. ote Y Sheba Facet Oya seen "gad eae Re Z esses ies 

"4 ' a %, 9 } S Maples wo ghees 8s kee % Ao vee aeze py ; a teare Savarese A, camsacasrteoyaeee 

5 du! dee Brite alsad des adhes 2& Dad a8 8 rh de MA yA eve ASR, Allee AD POT, Pamaeagayase, LEO 

: rn Y Ade MSTA INL Ae? > Or nee 8 MARLO Nites dP ms tee Botan? Qt Made wasuam ‘arene 
' - Fart 4 Vo Metra DOE P66, Abas ay “y) ei gpk RAGhe OPA as AG Aes & sug ngh p.16mg oM REAR SS LEAD Mg 

wo edo a7) eae 4ab = : Da AOE READ) RO, R. D0 CPs sah tt Peasy O60 OOD 
i at agate, eo 4 ten a* 9 Agar es eee " ue e AAGATeB Adan SMA eea RAs WB 7%. 018, ay ak 
ots oe 8 3 ‘6 SOND Bete pul y Aas vty oe £6 od “ie! a = § Aths. weRe Ge oe “tiAgoa MUR in Po ore 4 ted 
2 - * = ‘Bye RE Wtekb7 - BAIA UNAM ILO A. Vevey eat) tee Uy pad ieee. prerrry 
se o RQ el OU} UU ane a AMY 1 cave ~ ha Ca Alkan Uae AM EAM Ot820, 25810, Madde taorsareases eB Aeuiotes 


t ' ’ ‘ 3 é A, ere Ul ‘At acd OLA, t a hears halter eros Wot CF AFALAUE 894 ARMA. Fe PASAsovy pay Meats 
. . a . 2 wea ht Jo. ‘ wt # ! 7 ped ee Sita 2% ao ews gripe Fi Asadw, © Pom Veesere win Banna gah 

- } i i ‘ &. tsalete rh ey Pee ee Se eee a ee 48 20m, 

meh MAL 11j.0R 13s par FBT. 29 Ls AAG A ee wees, DMRS Vogel yit 0692 OPAC RAMOMaO+2 980 0 
Seti Oi toe = AeAsageemts As: es he Peasangattansegacie 

rire 2 aggre Syren omnia Thghe Bag tras? ane dim 8s OPP ARE Oo ane 





{ ey 
ects 1! * ' %» i tant ibe 
: : ‘ Biieteddeact Poppies 
. . ti, a a nie iC; peel we abuse Nhge pane 2 Hu eee PoMearl ee Boe AUBIN S85 Garay gore mon Ay A 
: j A atedts 9 wt : oy ar Hitec “8h AS Roh de, Op tae a ewbensyn cd ose: OAC. eee A papa ghimars +f Fane 
' 1 ’ 1 =! a eet re Moe. BRK o & Gm Psal ede dy 8 uasetuee wf wean: apainste en take s1hj Opn aor -5 Pernt! 
. . « gare 2 ta & \epip tee, Mr AGMSY SRS, AL Perk! Be, ae soecuie natateannny: ys QP ad OUD sed 6 sth 


t 
U “ s8 1 ai is | E hell a & v6 «94 pha. ‘Ss a 1.4, met Biers a zie ang welt Wt fare. Oy 
ie eR ar =~ “ie ay > Ye Pees tet > seh ie taken TH re es pal Crh meine, aa dpanaa 
7 2 i ‘ : 1 ofes he th » pales Ae he ae ieee erie Chore Ey reer rected P10. 614A ergs 
so bunegg as hgt ich pware ick Gute’ 8 feaitis cele tis 08 that / “Asas Vd at Great areratgnnaree me 
ae | a4 are tg tt “bibs id? ead ras. ihe sage Pee ere eT) yr whe “ts ag ise 29 %onm, Od Pies 
: o Yo. Aas oom sal ed QDR: BREA Woh Ie ate Saeaceyaet Haare ae. “nab 
oe et # “eee, elite ax Span Baer 0.8 th Sele MsAaas hovans peers Ales te Pee fet alles 
eg nn ed veut, (Aa Le iret ee at anata? : Tetara 8 Piste saosiens. gia ——- 
Vita dk dere. a ete pai yer iets ee ee «- i 6 Ps: I Pray oe 
vA ORE iene a 0 Sh Uae EA VIAN SAP iar RSIS R BRS Y M0 RL ep cdes 0048.9 70y 0.9 neh, 
! sie Arnica Cigale Bed Lek TAM. We. apa gus parents ancgnadnovey Aen 4IAPV ARE Fe 
y ghee ry vin fine adage rt a ee ALB.a08 w.Bdetind 8-4 08008 a RRS he Pin sas ASA PAsPAsAoy ohne 
ie ray, 3 ‘ase’ prone * athe wae. myspee\ Asis Vg. ta teRevrece en way leinsmg: 
HAy Spt We ndees Oy tat ori “Ble Dankestc arate Dew. Peng Ae 87O.00M) ACR, Bs RPL RAARA TAD RRGe 8) fp We 
vgn eh Te Sa8pne © dja pant nage bby eS oboe at a atree ae ee ue ge Mera te Pam Br a. ors My MEER te Ra beRe ae 8168: 
1 : e 45 fh anne ake ret ' rit + MR Lar ae A Ry hy ete 8 NO ASARD Be 21048 hee @ Doar yp srtitgy £00 Ms LA SORT 88 oom 
4 * oft % 1" genes! By Pe tah. + Yong ain, ours + brag Lammas seads HR Rs Rs 1a Oath F Goyt hi MO da0% eset aeos may 
ot ‘ ‘ mB akgage 9 ove tp wt > ag » iki AeA ise, OL Codd Oe t WSOe tA aad Beh dcn an yanon sn Darah 8 tan te de 
alg Lt. wpa ea o 2 oat se t's age BAA tnd GAs VAingarans ome Mase sarnary i Me he Ashe te hes? PApwemE®, 3 
ie eae ee Fash ao TS Ase ga aet Pe ir » SAPD oAS RPP s tt gue R ne Be pedhiare.§ Metget pera Sees 4 pk 
sat aa a Ah ee we TR ey SIP ate ee aa ¢ wna ss Ba ages ef ree are 






















2e 











= ae Ree: 5 H La atadary 
a 4 ' ' LT] 7-¢ = ale rere pede . acaec BS ti hem Bake sual ine Tr ® tom wenearacags gh eon Ey sree orb « iahae ana! abe 
» ’ etre O30 18 : “tata” be C0. 9 Ces sry aces wy On . teat rods Ade aad Beiter nous gig opera 
A '  . & b oi paameon Sears mh ® Mp brady stRghy ae cath ASMA MeN O ts 268. BaTiO vasarivasacwansapacene By menage ds 
L ry ® r z Men tg tad’ D. pane cf ie‘ a x a Sahn sh NNO ayant Peteet ote oe &, A9Q As Sete te Raden ee 
. ’ ost toe wh tippy 88 . af PY ee am ats *age © Uyat i: ee fe Yom e Wer os ary obeF oR eh ena. stie 
' ® nf Riles as ‘ ® ges a ty a» ora Py Aaa + 4 AP wan @ Remeen. £eden, o. AAde Orn crews IPS 89 ORY Ryde Rem v 
a) m4 an ee) ee eT 7: erek heetes Ook ? Aged rT ‘, a'AgLatyosnce t ae ne vanes aA eENT SU Poy Soaps rede 
Es Dal =~ am, ¢ vg Py ae giste gcse Far abe Paint | o™. ESN one agente souvent mgr ahenete pace ne 
tt ms § iv 4 ’ ee) = a 
* i aga ve te 4 A ave vtieys ENA iene ay ah eas. 19} N he ake ' vhs vines Sia 9h Wohi day an poste nchet + apie et 
i ® ’ 1 3th b ws phy te #agt va weRIIGAL MEAS 4,4 Fe rede aasig easmratacgnrtces as Mame M/A sit ta te tg tae Na: 
a a = s e% . : BANGS WLAZAWO, Wo wings NA, pte * + ta Ber Copan LTA 1 Ss sm adpe oal ti: Ieapels-t ore Sere 
‘ . 8 "a ” 1 hth. ae . R wy) . on omeeh. = Ay As Rs om 
x 2 & balls, 4 ST i iapret RH el ae au te tags ae pen pre, aR Se Ber yo. ofen wh a» pe OTR eat aa RATS: a 
aks a . dy > . A : mare 2 
‘ - i “ ry 4 ’ <a a Rae ay ie i phe nr ree id caane ly ts At Sse 6 ay rs a wea ae = 
- ’ * x seta oe % © Ath See ahve he i i > see Wa rogercanedi as! We reglee i adi 
i se pie wae Ag 08 “CAD sn se QP eho ' roy oa iSerolee shinai Alesrewges oS Qeee tds eiemt Sarsde we , 
‘ . ar a 4 ty th fog ao¥ bs ’ eu Ret et mg ong ht ti reder fepg oe Patio opgemnecse, <9e Bars Re tee banen, an Om: 
A M ~ 8s oa s* tee 6 *30¢ vs vad Ge Ret, mm a Lye ree ben ty aa he 8 Ae Ronin * 945 OM ogee plete 2 veers 
rn o4 ’ . Lad fa’ yt t-0urgeon a patnd gah Ate us - a Te Ske mageruse 2 Sep netamin MM agas ve baw o( Mr ae oy om 
" es? =e ob ages fei bs Oiu fy Sus. raven +s ern er nape Syston sy w 01% SS Poe na ty. be Oyiny ake 
; sh OL Be bs bau yo sie OLN masnacen nie Shcahen NEE Cae 
r Ose, We Esp unt Ais : Tykes terre) sagan ® dadion ot 
a we te) 1 * os. pele Fe 
Ke . ' peur t 13 3 ee the tee PRRs DwigS ve * matexte Ahem anipereaathe 
Y OV on ad ee ie red A wee gee pas en hae Rien Vie peed edie  dithoche Ye 7 tote tat 
ninlia zn, Ms ‘ 08 34 fijoar pt: 8 we » ORrhomrks mGg hi des ne tge® se ySivaes 
4 & q a0. hae 4 nt Jepada tye sory 14) Bethe gtste Re Seats mesa he 
dL tear =. wee ti’ A NAH tie Pee ma yreete WEE ee ayephen rh 
een a) a's hex ve Ode fie me pase! tr Qehted eee ne ag ot pert gene Nee nee 
rhe MLNS ee © fet ne sae “sh be Pua Ss % i ae do esaniaee boems ef a ae Seat 
‘ warested x . wer ad alb, toe, Stata 4 a 8 ean avgens ibe) ye 
2 ajas ak “cae fi a3 e5h0" rec BO ’ oy tee severe on aehs a 
a fe o> by ar "as ae Missy eal Aap SeerSe ov Te ste 
§ saat as aH in Asigt abe’ - 8 mad at: Sat ww Jhasrcgeued prriahdtncenig © 40h a=" at 
at “ghee Vasepee "es ib a, Sere Beet arte venta So rnyins 3 ome eps ar steed 
x Hue ate leah aA if ” Nac af 45 ee ba cigh DRAM eet. Saeety adhe! “Sete'e Aree cere ee 
vig tr 0 eRe Da Y, « . aw rst % lay ‘ wis? RSS ats are way er “eee ee 


> 
* 7 J ott, 'o® ap! ek, F Ame Ate 
ti the Byes 2 a 4 pt Ha . ee 27% we on eck: P| aes ta Bh - ate 


« recta 9 a at Ae 1 Tides’ Um we is rich teecrg eo of 
7 o ou : yh ryt ie be bo *y nebts ren eae Py ag Aue ve Rk: 


















Me be BOD 










agnene Mi a 





Sor. 
J 
> 



















* . er Seed . 
~ i an a a dee 2 4, ‘ed, ' aft ae vi aR ee wees Se Saag. ee 


s 

J ae ‘ byte Sasa’ Somme tt Fa mel 

' 1 - Sy = we , | ie oh % " dst PAS ay eye Benene’ a = acigegh et a MK cu rr +1 6 
’ Pay ¢ ‘py oa af ’ worn creed? HE ~fe ive ate esiart Nahas — | opts © jo enema aed 

a Tey ee 8 ut Pais o Mons % Bocse bart 4 tees’ “AV 4, ed ba ag vo teens ee 
' 2 Ly wre ai. Ge a a & vi aig? oh ge pes aery Fagg 9 ig 848 Fe 8 sa Os ot aatis 5 
3 a dpxgognne gfe” *s aig? Ly ss xe 3g 8 men Be head ds hey tb bee Sa wei 
f Fas at Ieee ee age 
on has 5,4. bee S20 & viens 
Rows -Aeteies he. ta tvst 






‘ vee we a 8 ad Se ten yh er moe ® 
i ' % ! a Tags fe oe Sa ' a? ‘ at wa ay senate + at ue Pr 
é tr es UJ ¢ ., yt i pee Mf =i, pase is . Peart eet 4.0, } 3h 

: vA. mt ssl sy, VRAAR Bt #5 ‘. a 
. F oNe% ; . ae ® ue aes $y: fod “. A me 23 Fe GE ape oe of tie 2 
‘ : —iree'h 5 te RS Pt ‘pe in. rd «3% oR isin Ne ktatate 
. = ~ 1, , ar | ng Xo i” “gate. Fy +r *£ Sise panes whe t LO Teas 
. R i ' : rit str ty, § 2 ‘s ces. i ett enti! 
. Be 5 i Linn . ent 4 f° Pt WAT 0s a= > © sx 
A us iw” Pes vs. bectes at 


? a 3 W = qd a --’ : ps 
= ib) 2 age eR, fh, he FR Yt Teagk uted de Meee Staite MM 
th” . ere x} . 3 fs . L 

























"a5 " t's & “nee? 
ered 


f li : 2 Dad Cad 


mt = Peewee Ste 
Sint of Mtacel be 
tt Rss; ite ey Se re, 


wots ck Sk Sees” ee =: Su. ri 

Pig ere Pee = PS wae ASt 

aes sg 3 corer 

5 ere tren See Sgizsd w iw 
ESSE Seis 5s Rye . sb ASE % 
5 we op Ss Beye ae Fee 

‘ cy : =. an 
vent: fi SF MT, ato agave & Arete 
C £,0° 4, PERCE 


j .ae ree 
AIT CaN 


7 J 
¥ 1 '— * 





” oben 


7 
‘ea 











Hi 


‘ 7! 
Aa itp an we 
1¢ 





’ “ a # es 
ial i on A tice “eo ’ ihe dew ae 
» * at; fp vie sa ‘ eee Otte 0 FE ah, eer oT ied haath = ; 
, 3 Ap Roe s iq rr MS, Cd sepa a] eres i rere om 
os + 108 yan eg bar era? “EET UN we ect gt mt fs 
rete oft vorhed care soe ae wr 8 Pata 
a Pht ate wm ce Be ise wee pt ste ORR netiee fied * 
es at nies pf oa & ass a oe 8, CN Bakes cet Rae poss oie a paw a 
prkiee i ee pi redu a, a sie, ‘des: eater 
Oa a é ~ ne wie 4) ae, as Sots ater ot eee 
' eM a = . r es eh .0 Ps abt re ere et see 
: ° 2 ine, aa Sens, os 
* é Py al Sin 8 ye a5 Pe My * Es, esto Agr bese gss saiyhoo vs 
7 Fi “ © + SG m ot & cris gt, ne: Pe eee ot Pe tee) ate erie age 1 Nd ee 
1 . 1 =) i an ¢ 1 we; ’ retin ted pe ne ETE 8 alg Brewin see eet teaee 18 3 Pye e pasa le ae 9 aoneee 
’ o i i = wet pie ae Aer 5 iia % od eee awe eles ge oicae w ah Jarek I’ 
8 ' ane reed Se Fr es ctaclaea' a3t soate a RTE Ce ewe eri deo, ¥ pies: eo 
1 ~ - "4 y 4 eg en Rie é. spon yer are rye gerd on daegesiorer el faba! # ricwrtee gS ai-g Orrate 
- ' 1 = i 4 AYRES I Be, Gye’ hey > shri Renee yt et ee at ee ad beeen eb! at) P 
i tyr =) = corber Wa gt tu Peery ae Hangs Pons Fa Sun ee Sore eengecr-« oat po Fela bd eae se 
a vite re ccs oT ee or on 2 FO! pens a iemagte Teh erly 
; — ye - oa \ Bue ys, nu ets Fo 0 owe bY abeppag al 8 
Py Vaan ” pretrce ; aa > Ry0° pao 0 are Jere + ’ eeeian- ie bea + 9? Fy Motes pens 
so " t , =s +t ww or ace 2 PPD oft fo ow 41 Fe 4s pepeersh: bape vit goatee Poe tall Utes CY aie yay et 9 OT aT Oe oe 
% ‘ee e ree Se 3 uy “Fi tb ai LET oa » ee ere + Peepe tn it feeb! | pu i dpe sf Tadd) leek ih cal 
7 1 é L ° Ms nes oe! . eS ‘ Arr Wi ASete 00, cod eyes Se hy ny? bt ae 8 90 ae oe ow 
f i i am + Se Oa) ant, + “a tgcveewers po graft Teg Deinbseudes ately? ont ep Rotate: 
% oer Bee 50 ot tee Sy pretties atl vpasin ny ges Sh "ot dew * am 
= bs afr ef ote 5 we ‘Salaacte wget gree Gary Ap nn teaeontane PR ohio Si to) Tien over x 
ay & se we f en ey P53 Bee i Santi Fo iE oho = en Reed 5 29™ te a? Saree ee 
| ae ¥ a ee eke he ok te oe ofr he a Yael Ut phate dod . PORE gre GF? 48 9 ete! 
I po ae ree “s Une ON Very siretye- Ae Zs ae hoe seer atynr YO Pome 
re Tae st Chicas o wh ore See BF SOL Ve 8 June ee, spe Tera eta gat 
fe : o rey F1ke Meee vg eer sapichine & Ff MD Psu 4018 GOYLTY PRM RV COTS O, etn ee fare oe 
i YW soy Gay He ae tn ah Se Oca ate eo 8, ere iga EO ot vhte HeatrrrAN ermine Myasee Poo im ge pam Pht wee PE OORT Ft 
e wr bs ere te 6 Lee De 16 008 greene gees need» wel cata Peg ie ct oF oF S41 i8 ite ene ge 
“ ' « - 6 40 ' + ies Gs. ot Gey wee eae =o tiviae ory ere Shlret ayaa. PORT wet pte SMe eee) eae woe . 
> or jrie wort evenanet o eet 2 BL ALO y WN 180 NW Sistas is FS Foie ty 9A Le Ai Tom a1 bed oye Swe? Oe 
i 1 SS = is Din a dyive s party onteg toa ee oeaarer, we po nS g le es wae ers ce a heap “ 
. 1s? € =) + vis ’ aT bidoed Phased Bes Hit AN MP yes, paper sa ateeli cas Ort w at Py at om Fy oe ey EWE OFM 
ee a X Neue —w: ad I eee 6 eee tate en ell tt Tag Fitenope eI we s | Le DATO ON IO 
' b oan fe Area = inne ed ee CL Ss Cl anes e Ee Lae fl eens ett ramel oe 
: fe Sala ‘ sot: atte, pditcet pteteres, -avgeanenns te He pcb peaerntartiy 28 ae pWo¥a1 O° RO-cm ¥' 93 Omer dO a 08 dey arene 
< eT ow - oaey . Web FO ee RY sears. 2 AROS): PAUMD opmyarst ipa arrose ODE 8 Ce OO, GA OF Speyer car 
, He ‘hei on 2 “EFF apes fo Te te Te es ee py onenbeetrenaetpn te ria path eg bes prema 
, 




























& of tered > hte primes 62D perlite 4 ath Ore abt h nila pee ret 1s pet fi yw Bataarasers— ie 
Vs stm ph FRING ok Baty eda 417 Up HOVE pos pew ot nrtor at ae sri ranted Heap Uric tatintnte weer 
. we Cay he 9 oP Ger! aaqepet tt ah arepat epee 08 spelen My ad 947 Satan creae SOP Sig wt oe ‘ite rit venee errata Agel ge oI 
é ’ ae? 4 ' oh btety. wis ese My ont Mee te ie Ste cine” wo relist atta Fiyewr’ Uitte? a We eROe et) weil sy oes ape FTES T et va pt al OO OA 
ee yes * wee WTA ons Sa qreray wore ne eo wwe gs ape Byes oA ewen at wages eeeee weny7 ete’ Saag Pe, 6° ayer" ps1 
La “ ft Win POF Em 8S oe Bylot aly ae Bile WORM WPA ese bee oe aegunae’ piiceoates 
i _? orto oa Bovinven’  sasays a8 2 Ha ee oer tare ative Were! ntteser 2 Aee Sa G'e wr oral irre se a es 
amy" 1 nantes 17 LE ke pee, Fa a Tt ieht ok “egies ree of cpt. uowonse ce? Saelene! oss te gre etareetiSal 
eed ere erigeae ok gee, ibenes* #00; aig o% t Fuitacsiane’ «! aes auc yesreme ween ese seven pandas aad awe otou nto SE! 
ry , © % «Vl eS ud 5 Sh co eters CRI Stet Sale! w sf? Fete CHW Ae Ve este rwamenes tale ave. PUN GY globe pee 2 Tee empse noe 
% g iat wer 4 EEOPLO He ots & RIPRO  ie =a CAA Hp eoee Wo o5R Liebe pi Pree pete ote: voted jai tregaternre & gue > ae 
a * a Otter * ® nil =i aperture La Ae edy se ee Dstt analien Wath) lag PP as” Fae eee twister ws die pe ee eee 7) oye 
A ad hy bs ca Sonn: ARAL “ Geo pheiutgree Fr  aiaiing lie POR 8 rata SN dece ee ld paagarann PEF GE FOES ame 
, © it ee ad pilin 1 taege 1 tt Pit vue a Boe al owe Pad 13s i 1% vide © rhe alee et os rpriays gone. on waetahes’ coast orcaiean tienes wreteae 
! eae Ae wie sais 6 ies Ps a Pytany aeteties Argel Feta Ree wines 16 pase f8:Viye ean? Se naehan 
i we x* ce 4 pi Pg ay pgs em teas ea a] op yey eee ts ah VOW, nae RO i tet ee id eee eh 
ry : &* ry, Meh, Gyre ha we? ota went ied Be crtsetaneaat st Taiearotye eo Pert cw ererQmreytrey 
“ ' ‘ y 14t Bg ts mad veg S04 seb ines vy oAZ0 OOO, 700 Orgy vee dwar 0 geet pra easuco wbabeness ng oie FHS 0s ahisCPegs ation =r area! 
. i OPese aie &, > Rests, ot " ve te ae bole wee tne Laverton Ob OP. ERIE EE OW PROM TH GY Goethe Pam 
¥ prs - Le whee meayoeeees Dali ad ri papestat stat ride at hut be Gots were -poqaerne wyemmcomerauehs 

a. Pah eee meen eer whae 





i bas na ssi os at eae a entail. dl pili 

a? St. a3 gm ree we "Get ite? e4twrrvn s eye gg ry Reagan sito! ae Srariteeay RT Ora ee Vener = 99 we 

Y : * “4 WN an om > Soe 04" 0 neat Mla y-'-"lgd vb S49" Spe Sapte FA? 9% G9 0 Or pittee ili pe Rewer sy" H 

196 gent os “Ne oye Sy beac arae rae hi meat cd aR FPR ae epee e PRO Ne +) va. wrideaee fone Sins 9e PhS 

E. VER 198 attain he imiecrehenr oye ate sys Hie sr ) eer 208 tae mo gemrerser ets 

’ ae +. BOL opoyd Aa oy jf eo hcl tae a mpatinies riaae oS-0 Sass nee mipeves Amy wed Gaede pice alt a 

f . . r oe. = “a — yt 3h AD aged opt erhulgs oreere, — "ye%et ote Mtl ae as 34° pip eho tee NOS phews? gue aie mere 

1 Taw ort ' ft == wey tps = Sta aes eg < aes ae se Bsa ee me rept ote 0% fiona orn vot vernewts teeatowiners 

wet =s toe : “= ore Sgt yovr swedwst by Sy eoratea 6 owe a ges ae FRU Ve Me GF Flere ma yem et een aM 4 

alll Pan i ned oe Ener aut srse ptt et ae An or ° Saye ahbisias sOtwi2g. veep ouravede“vowrencqwit voll as el CUE ed el ih oe a Ld he 

.% baatieod at 1 We gk OR rlgrs Bs ye eg wg bat "Aer sche pany’ PUT Oar OTIS MAU OVE ET? pT YES pS 7g: whe Che ow ERR etU aT 

zrupe 2% 5 Sane ares Par. tas wea vecuigt a 7 on $59 MTN CO Me ve 9 S88 PEYOTE Ee iwerin Fret 0 0 tear eis 1 

r a ' ‘ £0 apt renyten= gi vin egy nese See oie +e Oe \ barbnaedg HORE REIN Pres! £640 D~ ery, Cee etter both Cliche riba 

i . 5 et ow es "at rd Reka § 1% Ae yrengress- aie er bag acta HS stwep! sews, noravenfice severe 
u a so “ad ists a a trawy el votes Bi aE sag'y b EOF vm 04 Geqr SRA rye 16 eg teen VEr (A94 amentgn-o ponte 
a i daa) Agente at a wiatess se OPE FEET LRT OLY NE YT bt SE Fe pe Red HP OF GAO wetodns D190 .07 
re ie c... men was rinkt reais “tie roe ean FaGr re y= ba Tet Sioa can 

’ u peo ne “U1as Calg o! 4 ave Mrgisee tye Sy ate wah S90 EP twrelaaee yy Cat tl alnad talk dental 

Lae int Av eer orlcie ones phi ' aes Lied aol "ene ee Shy pas rveres inven great 
4 > “7? i "eegrhe gt Hees ep Abe cet mF Auer etry (eatoere* ET Et PAY Y pre ereran wry? s 

. . € a ree wires oe poy ot 8 07 49 econ duncnn tote Lae bs ee ve br Top peepee hipaa 

ay = it “nee Tye orn bP tt! at mee by es GOR wes wes Fee 52974". Pees Do Se PE DOE Ge 6 HE aT P CRO 7° 
1 a ers “~~ are Monger Ae Ged | ny D> RHO H ore vythianaiwannancanat 

OCE. cee ees -_ = m> . ¥ iyte- ar’ ars seed ie ey area OT era Pyoeew geegrr, 


re wow ALE - 
ms ‘= mp ree ive se Paria? Pi Gain ethene Pinte AHN led hahah 0. ee AT 
\ ‘¢ oT iva pn rie Ges aS ey eh ey emat “re ony arise we noe 
: . s aie ay UP ier fi nh -, Pemoes, aw eles poe eee eee 
’ + : ee eau ego cit as ae Th ancwir, bre 
7 i . oy se Owe Mates att espe 8 Cory. PIETUR HE or EE © meee mee mere? ACE payers ra rGeL 
' LY = Ee | GOS Cl ty aeereep Je myearer rine eca net ry sve ier 2%) (A002 NSP Heroes rary POAT Sheth at dod bpd peadoes sire 
4 r pet ‘@ Bag ss vee (Sane Sibaget atahieee Sates ee oe gt savin sonore Be per 
. " Le shontse@e bingh tts Jal 

* ‘ o ee wi “et an eetgtios Fee lag 5 1 Ole eat aes bafhlla ape ipaariee ametneeneeny 1 pate Eo hapedddhedod 

i es a a M tard : 99 cco S1lby Ce yeprea ins Tea ttieretomreerte vada! re weyen ohtee 
Ld ° wets 8 ee deere "© #1 te " so rhea’ aye ce re ree, Cd Bde Pivape miagieee gratarere pies Lyell mec re 

i ths ed a! it~ hoa det ad La “od bapa {oograreseracyrtv avy Cray ee be tl ten hdacad Lal Pape to 

. oti ‘ rat . ne Vo, Prat ne sok ae E46 re ooveren: GE Eee Ber? 9 * Parnoul WAVE 
' a 3 nL i Ld a rie’ a Aca ati woteg st at ga fy Hein ate i eh vrpedeceety-at — ae we: Pea en ireamensianeree st ai Wi 
Owl Or qoe | od asades bd Ut aa) ae ee 
ten run on Prapepiveununn ye ee wor U woe baipormesrir ceeds ye 

APRA ACTIN CM 16 BRAVO Pod FINAN F104! Jory CR bain helt 

, v oat § are < of pore tes “Ft i Behe ENHG 1h TIlE VEN eS for ae) Ye Oe 988 Bee Ree: oT Os HOF H prET 
ea ' ny RUMEN Ege Pye OAS WEY EN pever ise nent at rivureverv yarns: weitays Web Rat agent ep aru oorsiarn ntti V's (a8 Pet 
, oo a = PRR ey ie te ae a RY WI yu oleae al Ata on et TERY STOMP TST Fhe Hewes 
Si : : al : 








= hg pet es i ah 


- 
F 
. 


" “ | 














we i. mt L Sinet oe ve re ete oreneee: 


» 
} ? 
~ 
. ” Rpm fave, “trecanarenars is aie ph Abed thised Pehl b-bbgrst 
ad ‘ hd hes Aicgeyw Aled 4d bh ole aed 
F Z 2 “S ee a pried ek alia teresa ows ee Pa Take pamper o rat 
vi a re “6 apd rs Be + les st fess sor sete 
Me 


as ~~ =- pot eretoatpe ge oat i da 2 acer ie 


ae a 
_ i bag . 14 
-_ J ad piace ie é ted me ates wre veseaee iA 
ws fe “sited daa ool oaba S ° « 
° ' Be a = = wave Ee SETA ite enetnennrwrnn ory " we a “A abs Oe 2 Ee ae teh ln 


va as =a: 2% we? Sve Saree rraeats ev Piles pe geet SOE nD ay ras 3e79 h 
ares 











NPS69-88-007 


NAVAL POSTGRADUATE SCHOOL 
Monterey , California 








THESIS 


LOCAL PATH PLANNING USING OPTIMAL 
CONTROL TECHNIQUES 


by 
Winston Smith 


June 1988 


Thesis Advisor: David L. Smith 





Approved for public release; distribution unlimited 


Prepared for: Naval Postgraduate School 
Monterey, California 93943-5000 


NAVAL POSTGRADUATE SCHOOL 
Monterey, CA 93943 


Rear Admiral R. C. Austin Kneale T. Marshall 
Superintendent Acting Provost 


This thesis was prepared in conjection with research sponsored by the 
Arctic Submarine Laboratory, Naval Ocean Systems Center, San Diego, California, 
and funded by the Naval Postgraduate School. Reproduction of all or part of 
this report is authorized. 


Released by: 


ECURITY CLASSIFICATION OF THIS PAGE 


REPORT DOCUMENTATION PAGE 


la REPORT SECURITY CLASSIFICATION Yo RESTRICTIVE MARKINGS 
UMNCLASSTETED 


Ja. SECURITY CLASSIFICATION AUTHORITY 3 DISTRIBUTION / AVAILABILITY OF REPORT 
Poptevea + or public release: 
Peser i bDuUtaON unlimited 







’b. DECLASSIFICATION / DOWNGRADING SCHEDULE 


1. PERFORMING ORGANIZATION REPORT NUMBER(S) 5S MONITORING ORGANIZATION REPORT NUMBER(S) 


NPS69-88-007 


»@ NAME OF PERFORMING ORGANIZATION COT OFFICE SYMBOL 


Naval Postgraduate School (If applicable) 


7a NAME OF MONITORING ORGANIZATION 
Naval Postgraduate School 





| 
1. ADDRESS (City, State, and Z/P Coge) ; 70 ADDRESS (City, State, and ZIP Code) 
. 


Monterey, California 93943-5000 Monterey, California 93943-5000 




















a. NAME OF FUNDING, SPONSORING 9 PROCUREMENT INSTRUMENT IDENTIFICATION NUMBER 
ORGANIZATION 


Naval Postgraduate School O&MN Direct Funding 


c. ADDRESS (City, State, and Z/P Code) 10 SOURCE OF FUNDING NUMBERS 


PROGRAM PROJECT WORK UNIT 
Monterey, California 93943-5000 ELEMENT NO NO ACCESSION NO 


8b OFFICE SYMBOL 
(if applicable) 













1. TITLE (Include Security Classification) . ; 
ocal Path Planning using Optimal Control Techniques 
BeVeeASSIFIED 


2 PERSONAL AUTHOR(S) 
} le iB mo 


3a TYPE OF REPORT 13b TIME COVERED 13 DATE OF REPORT (Year, Month, Day) $15 PAGE COUNT 
Maste hesi A en a 109 


6. SUPPLEMENTARY NOTATION "The views expressed in this thesis are those of the author 
meeco mot reflect he official policy or position of the Department of Defense 
ee tne U. Se Government." 
? COSAT CODES 









18 SUBLECT TERMS (Continue on reverse if necessary and toentify by block number) 


FIELD SUB-GROUP Path planning, obstacle avoidance, autonomous 
eee} vehicle maneuvering 


9 ABSTRACT (Continue on reverse if necessary and identify by biock number) 


The ability of an autonomous vehicle control system to plan a safe, collision 
free local path from one vehicle position to another is one of the most 
important functions. In this thesis, it is shown how a safe obstacle-free 
local path can be planned using optimal control theory and optimization 
techniques. The problem is posed as a two point boundary value problem 

vith various problem constraints which control the vehcile behavior in 
Cransversing from one paint to another. The objective function being 
minimized is a control performance index which includes vehicle energy saving 
Jarameters,. Numerous fixed and moving obstacles in the dive plane are 
Lntroduced and successfully avoided using this technique. Three=dimensional 
bath planning is also successfully demonstrated on a 12 state linear model of 
an underwater vehicle. This technique is shown to be a feasible method for 

a local path planning apslications. 


) 


eel OF ABSTRACT 21 ABSTRACT SECURITY CLASSIFICATION 
Muncrassipigovunuimiteo OC) same as ret CJoncusers | UNCLASSIFIED 


‘a NAME OF RESPONSIBLE INDIVIDUAL 22b TELEPHONE (incluge Area Code) } 22c OFFICE SYMBOL 
Nayic c 408-646-3383 695m 
D FORM 1473, 84 MarR 83 APR ed.tion may be used until exnausted 


SECURITY CLASSIFICATION OF THIS PAGE 
| Ali otner editions are obsolete 


@2@uUS Government Printing Oftice 19866—-606-24) 


1 
4 


Approved for public release; distribution unlimited 


Local Path Planning Using Optimal 
Control Techniques 


by 


Winston Smith 
Lieutenant, United States Navy 
B.S., University of Mississippi, 1980 


Submitted in partial fulfillment of the 
requirements for the degree of 


MASTER OF SCIENCE IN MECHANICAL ENGINEERING 
from the 


NAVAL POSTGRADUATE SCHOOL 
June 1988 


ABSTRACT 


The ability of an autonomous vehicle control system to 
plan a safe, collision-free local path from one vehicle 
position to another is one of the most important 
functions. In this thesis, it is shown how a safe 
obstacle-free local path can be planned uSing optimal 
control theory and optimization techniques. The problem 
is posed as a two point boundary value problem with 
various problem constraints which control the vehicle 
behavior in transversing from one point to another. The 
objective function being minimized is a control 
performance index which includes vehicle energy saving 
parameters. Numerous fixed and moving obstacles in the 
dive plane are introduced and successfully avoided using 
this technique. Three dimensional path planning is also 
successfully demonstrated on a 12 state linear model of an 
underwater vehicle. This technique is shown to be a 


feasible method for local path planning applications. 
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THESIS DISCLAIMER 


The reader is cautioned that computer programs 
developed in this research may not have been exercised for 
all cases of interest. While every effort has been made, 
within the time available, to ensure that the programs are 
free of computational and logic errors, they can not be 
considered validated. Any application of these programs 
without additional verification is at the risk of the 


user. 
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I. INTRODUCTION 


A. GENERAL 

The presently forecast missions of an Autonomous 
Underwater Vehicle (AUV) vary in scope from mine detection 
and avoidance to surveying the bottom of oceans. Further, 
it is expected that many of these missions will be 
conducted within the context of military objectives. 
Admiral William H. Rowden, Commander Naval Sea Systems 
Command stated that, "With the NAVSEA (Naval Sea System 
Command) Integrated Robotics Program about to enter its 
fifth year of existence, it seems appropriate to look back 
and ahead to establish a baseline for the promulgation of 
policy guidelines to facilitate the continuing evolution 
of this important program." [Ref. 1] He goes on to say 
that the time has come to incorporate the value of 
robotics and automation into the Navy’s expanding mission. 
Recent articles of Military Robotics [Refs. 2-6], have 
pointed out the increased availability of robotic 
vehicles. These include Remotely Piloted Aircraft, 
Unmanned Submarines, Teleoperated Combat Vehicles, Cruise 
Missiles and Teleoperated and Autonomous Weapons. 

An extremely important part of the total AUV vehicle 
control logic is its need to plan and execute a safe 


passage in the undersea environment. Local path planning 


is the function provided by an intelligent system, which 
determines safe, collision-free trajectory of travel 
between two points, a start point and a target point, fora 
specific time lapse. One possible total system block 
diagram that shows how the local path planner could be 
interfaced, is shown in Figure 1.1. Here, the Global 
Planning System would provide the Local Path Planner with a 
series of data sets. Included in the data sets would be 
destination position, destination time, start position, 
start time, obstacles and boundaries. In return, the path 
planner would provide an optimal path based upon the 
limitations of the vehicle dynamics, power plant 
efficiency, obstacle field, and required maneuver time. 

Numerous techniques have been used to achieve collision 
free local paths for various vehicle types and 
manipulators. These include graphical search methods [Refs 
7-11], potential field methods [Refs. 12-16] and optimal 
control theory [Refs. 17, 18]. This thesis is concerned 
with developing a method of autonomous planning using 


Opelmat control theory. 


B. PREVIOUS WORK 

A basic investigation of local path planning was 
previously conducted using optimal control theory 
(Ref. 19]. In that study, major emphasis was placed on the 


solution of a SISO (Single Input Single Output) problem, a 
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MIMO (Multiple Input Multiple Output) problem and its 
generalization to a submersible. That work included 
objective function determination, integration method 
studies, linear versus nonlinear solution results, 
computational expense and an obstacle avoidance solution 
with one fixed obstacle. The objective function used for 
optimization was a quadratic performance index of the form: 


J = es (xtox + uTRu)dt 


wm? Za i? ~~ re Pe 


where, 


U = the control vector; and 
X = desired states-actual states (i.e. state error) 


The nonlinear hydrodynamic equations of motion for the 
Autonomous Underwater Vehicle being studied were of the 
following form: 

MX + £(X, X) = g(U) 
The "best" solueven ae Sptaincdeee minimizing the 
objective function (J) in order to find the best U(t) and 
X(t) values. : 
: The Automatic Design Synthesis (ADS) Fortran Program 
(Ref. 20] was utilized for problem optimization and the 
Dynamic Simulation Language (DSL) Program [Ref. 21] was 


utilized for objective function calculations and 


integrations of the vehicle dynamic equations. These 


software programs were made to be interactive and now 
perform as one software package (Ref. 22]. The combined 
package is called ADSL and has been incorporated on the IBM 
3033 Mainframe Computer System at the Naval Postgraduate 
School. The basic optimization approach was as follows: 


1. Discretize the control vector into a time-wise 
uniform distribution of control signals (Figure i.2) 


Design Variable, DVI 
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Figure 1.2 Discrete vs. Continuous Controls 
2. Determine the best control sequence via an 


optimization routine based upon the objective 
function and problem constraints. 


In the two-dimensional problem (dive plane only), the 


vehicle was ordered to achieve an ordered depth of 17.425 


feet using minimum bow and stern plane deflections. 
Additionally, the vehicle was required to have a minimum 
pitch angle at its final end condition. The control vector 


(U) was the bow and stern plane angles, while the X vector 


o~! ow 


was the x and z positions of the vehicle and velocities in 


the x and z directions. 


C. AIM OF THE PRESENT STUDY 

This thesis is concerned with furthering the 
understanding of local path planning using optimal control 
theory. The purpose of this work is to: 


1. Further develop the planning level control logic 
to consider three-dimensional maneuvers, and 


2. Evaluate the performance of this logic. 


II. METHOD OF APPROACH 


The basic approach was as follows: 

1. Improve the treatment of obstacles, both fixed and 
moving in the two-dimensional problen. 

2. Determine the best set of optimization program 
options based on computational cost, robustness, 
flexibility and solution accuracy in the two-dimensional 
problem. 

3. Select guidance for maneuvering time (FINTIM) and 
determine how it effects problem solution in the two- 
dimensional problem. 

4. Evaluate two dimensional versus three dimensional 
computational costs and accuracy. 

The basic assumption in this study was that the work 
previously done [Ref. 19] remained valid. Specifically, 
that the integration method selected, the step size, 
objective function, number of design variables and 


optimization program options remained relevant. 


III. OBSTACLE AVOIDANCE 


The approach previously presented [Ref. 19] was to 
compute the distance to the obstacle at ten equally divided 
time intervals from start time to the time of closest 
obstacle approach. These updated distances were then 
incorporated into the optimization algorithm for constraint 
value determination. ADSL placed constraint equations into 
the algorithm in the form: 

G5 (k) = 0 ] = lan 
which in the actual program is: 
Gk(k) = (avoidance zone) - (updated vehicle distance) 
The time interval for distance calculations was determined 
based on the FINTIM and clock time as follows: 

If (time.ge.0.0.and.le.xobs/u) then 

timel = xobs/u 

qn = time/(timel/10. - delt/i0000.) 

Qo= ant (qnet!) 


dist(d) = sgqrt((xpos-xobs) x (zpos-zobs) ) 


g(d) = 1. - dist(d) 

where: 
time = DSL clock time 
xobs = x position of the obstacle 
delt = integration time step interval 
xpos = x position of the vehicle 


u = vehicle velocity in the x direction 


dist(d) = computed distance from the vehicle to the 
obstacle 

ZpoS = zZ position of the vehicle 

zobs = z position of the obstacle 

g(d) = constraint value placed in optimization routine 


The problem with this approach is that the further an 
obstacle is from the start position, the longer the time 
intervals become for obstacle distance calculations. This 
is satisfactory for a single fixed obstacle but for 
multiple obstacles, this method results in inadequate 
distance computations. This is because the closer 
obstacles do not have sufficient constraint inputs compared 
to the distant obstacles. As a result, the distant 
obstacles tend to dominate the solution. Using multiple 
"if" statements in this logic is also computationally 
expensive and failed when used with three or more 
obstacles. 

Another inherent problem was that distances were not 
computed after the time of closest approach. This 
sometimes resulted in maneuvers with distances to the 
obstacle that violated the avoidance zone regions after the 
first time of closest approach. 

A better method is to continuously compute distances to 
the obstacle, independently of FINTIM, but dependent on 


FINTIM step intervals. This approach worked very well and 


was adopted for all further analysis. An additional 
advantage to this approach is that only one constraint 
assignment was needed for each obstacle vice ten. 

The computational time for obstacle avoidance varies as 
the number of obstacles increases. The motivation for this 
study was to determine if this approach was computationally 
too expensive to remain as a viable approach. Figure 3.1 
shows the computational cost for one to seventeen fixed 
obstacles. The computational time is based on the virtual 
machine time for the IBM 3033 system at the Naval 
Postgraduate School. In all cases, the final depth was the 
desired depth of 17.425 feet. 

Various optimization techniques have various 
computational costs; however, the times in Figure 3.1 are 
based upon the final optimization option selected for this 
thesis. The selection criteria with results will be 


presented in the following chapter. 
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IV. OPTIMIZATION OPTIONS 


The ADS (Advanced Design Synthesis) Program allows for 
the selection of numerous optimization techniques for 
problem solution. There are three levels by which to 
select a particular technique. The three levels are the 
strategy level, optimizer level and the one-dimensional 
search level. Table 1 lists the various levels and the 
various methods contained in each. Figure 4.1 identifies 
the large number of possible algorithm combinations 
allowed. Vanderplaats provides a detailed discussion of 


the various methods and algorithms in Reference 23. 


A. CLUSTERED FIXED OBSTACLE TEST 

In order to be effective as a path planning algorithn, 
it is necessary for the program option to be robust and 
flexible enough to solve problems involving numerous fixed 
obstacles as well as moving obstacles. Therefore, an 
initial test was conducted where the number of obstacles in 
the vehicle’s path were varied from one to seventeen. 
Program option 057 was selected first based upon the 
recommendation of the previous study, which involved one 
fixed obstacle in the vehicle’s path. Option 057 appeared 
acceptable until a four obstacle field was encountered. At 


this point, the option failed to reach an adequate 


Ne 





LOrt OPTIMIZER 


Fletcher-Reeves 

Davidon-Fletcher-Powell (DFP) 
Broydon-Fletcher-Goldfarb-Shanno (BFGS) 
Method of Feasible Directions 

Modified Method of Feasible Directions 


OWN 


Ww 
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STRATEGY ISTRAT IOPT 1 2 


None 

SUMT, Exterior 

SUMT, Linear Extended Interior 
SUMT, Quadratic Extenden Interior 
SUMT, Cubic Extended Interior 
Augmented Lagrange Multiplier Meth. 
Sequential Linear Programming 
Method of Centers 

Sequential Quadratic Programming 
Sequential Convex Programming 
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ONE-DIMENSIONAL SEARCH LONED 


Golden Section Method 

Golden Section + Polynomial 
Polynomial Interpolation (bounded) 
Polynomial Extrapolation 

Golden Section Method 

Golden Section + Polynomial 
Polynomial Interpolation (bounded) 
Polynomial Extrapolation 
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NOTE: An X denotes an allowed combination of algorithms. 


Figure 4.1 Allowable ADS Algorithm Combinations 


tS 


un 


~-«MM MOTO OO *K 


xx «KM MOOO Oo 


TABLE 1. ADS LEVEL OPTIONS 


STRATEGY (ISRRAT) 


0 


ds 
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None 

SUMT, Exterior Penalty Function 
SUMT, Linear Extended Interior 

SUMT, Quadratic Extended Interior 
Cubic Extended Interior 

Augmented Lagrange Multiplier Method 
Sequential Linear Programming 
Method of Centers 

Sequential Quadratic Programming 


Sequential Convex Programming 


OPTIMIZER (IOPT) 


1 


2 


Fletcher-Reeves 
Davidon-Fletcher-Powell (DFP) 
Broydon-Fletcher-Golfarb-Shanno (BFGS) 
Method of Feasible Directions 


Modified Method of Feasible Directions 


-DIMENSIONAL SEARCH (IONED) 


Golden Section Method 
Golden Section and Polynomial 
Polynomial Interpolation, bounded 


Polynomial Extrapolation 
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Golden Section Method 
Golden Section and Polynomial 
Polynomial Interpolation, bounded 


Polynomial Extrapolation 


13 


solution. Option 133 was then selected based upon Olson’s 
work [Ref. 22]. Option 133 is more robust than option 057 
and it appeared to be very well suited for this problem 
until the ten obstacle field was encountered. This method 
achieved the correct ordered depth; however, it violated 
many of the obstacle avoidance zones. It was apparent, at 
this point, that all program options would have to be 
tested in order to determine the most appropriate 
algorithm. 

A test was conducted to determine if any of the one 
hundred and twelve program options could solve a seventeen 
obstacle problem. The complete test problem required the 
program option to solve a seventeen obstacle field problem 
with FINTIM set at seven seconds and an ordered depth of 
17.425 feet. Each obstacle had a one foot radius avoidance 


zone. The results of that study are presented in Table 2. 


16 





TABLE 2. 17 OBSTACLE TEST RESULT 
PROGRAM OPTION COMPUTATIONAL COST DEPTH 
(sec) (feet) 
Bab), 87492 7.425 
312 92.56 17.425 
3:3 58278 i425 
314 UeSAok Mecca 2S 
321 146.23 17.425 
Sze V42 32 ISA 2S 
323 je ogre it 7A 25 
324 165.12 1; Bes 
381 67.25 17.425 
S22 136.84 7425 
B35 151.90 ge 425 
334 120.78 17 .a25 
411 76.19 17.425 
412 96.00 17.421 
413 42.99 17st 5 
414 64.92 17.424 
421 11l6s6n l7e425 
422 140.87 17.425 
423 82.18 17.425 
424 94.39 L742 5 
431 i SiGe. 17.425 
432 174 35.5.0 17.425 
433 81.64 17.425 
434 97.29 ae 
512 70017 17.425 
534 48.87 17.449 


Of the one hundred and twelve program options, only twenty 
Six achieved a correct solution. The computational time 
varied significantly among the various successful program 
options. In some cases, the exact depth was not achieved; 
however, all results were considered excellent. 


After determining which algorithms could solve the 


seventeen obstacle problem, it was then necessary to verify 


nat 


that cases involving various obstacle combinations from one 
to sixteen were solvable by these methods. It may seem 
intuitively obvious that if an algorithm can achieve a 
solution involving seventeen obstacles that it can solve 
all other cases from one obstacle to sixteen obstacles. 
Contrary to intuition, this is not the case. For example, 
program option 313, which had a relatively small 
computational cost, successfully solved the seventeen 
obstacle problem but failed to achieve the correct depth 
when an eleven obstacle field was encountered. 

In conducting the varying fixed obstacle investigation, 
obstacles were purposefully placed in various positions in 
the field in order to ensure that obstacle position had no 
negative effect upon the problem solution. This was 
Significant because program option 057 (method chosen in 
the previous study), failed when it encountered an obstacle 
field with three fixed obstacles. Two obstacles were 
placed in the vehicle’s path and one was placed far from 
the vehicle’s path. The obstacle far away from the 
vehicle’s path was determined to be the cause of failure 
because the algorithm successfully solved a problem with 
three obstacles when all three were placed near the 
vehicle’s path. Using the cases of one to seventeen 
obstacles, the cases were further reduced from 26 to 22. 


Program options 313, 413, 534, and 314 were eliminated. 


lie. 


B. IMPOSSIBLE FIELD TEST 

After an investigation of the varying obstacle test, 
the reduced list of programs were subjected to an 
impossible problem. Four obstacles were placed in the 
vehicle’s path with nine, five, three, and six feet radii. 
They were placed in such a way that the algorithm could not 
achieve the correct solution in the allotted time. It is 
important to point out that a correct solution would have 
been obtainable if the simulation time was increased. The 
motivation for this study was to determine the failure 
modes of various algorithms. It was evident from this 
study that some algorithms, namely those which employed a 
strategy of Sequential Unconstrained Minimization using the 
Cubic Extended Interior Penalty Function Method, were more 
sensitive to achieving the desired depth constraints, when 
they were imposed as equality constraints. The algorithms 
which employed the strategy of Sequential Unconstrained 
Minimization using the Quadratic Extended Interior Penalty 
Function Method, were more sensitive in avoiding obstacle 
avoidance zones when they were imposed as inequality 
constraints. Figure 4.2 illustrates the performance of 
three different algorithms in solving this problem. Of the 
algorithms with relatively small computational costs, 
program option 311 did the best job of avoiding the 


avoidance zones. 
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With the impossible field test, computational cost 
uniformly increased compared to the four obstacle problem 
with smaller avoidance zones. Program option 311 had a 
computational cost of 74.30 Virtual Machine second in the 
impossible problem; however, with four obstacles it took 21 
seconds (Figure 3.1). Figure 4.3 is the solution result 
obtained if FINTIM is increased to 15.0. Although FINTIM 
was more than doubled, the computational cost did not 
Significantly increase. With FINTIM set to 15.0, the 


virtual machine time was 76.51 seconds. 


C. SELECTION RESULT 

Program option 311 with a strategy of Sequential 
Unconstrained Minimization using the Quadratic Extended 
Interior Penalty Function Method; an Optimizer using the 
Fletcher-Reeves algorithm and a one-dimensional search 
method using the Golden Section Method was chosen as the 
best algorithm. It was selected because it had the least 
computational cost of any algorithm which could solve the 
seventeen obstacle test problem and was very sensitive to 
the obstacle avoidance zones. In other words, it proved to 
be very good at finding a safe, collision-free path between 


the start condition and the end condition. 
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V. EVALUATION OF MANEUVERING TIME (FINTIM) 


Qualitatively, there are two possible FINTIM effects. 
Those which are associated with a small FINTIM and those 
which are associated with an excessively large FINTIM. The 
net effect of too small a FINTIM is an over constraining of 
the problem, which leads to a violation of problem 
constraints and excessive computational time. 

Two things happen when the FINTIM is too large. The 
solution adheres more to problem constraints and the 
computational cost decreases. The mission objectives of 
the vehicle (l.e., loitering at start position), are 
Significant considerations, which FINTIM selection must 
take into account. Therefore, FINTIM is a critical 
parameter which effects problem solution and also, when 
chosen correctly, significantly reduces computer 
computational costs. The selection of FINTIM poses an 
important problem which requires solving in view of high- 
level vehicle objectives. 

One guideline for selecting FINTIM is to select it 
based on the time required to achieve a solution while 
transversing an obstacle-free field, then arbitrarily 
increase FINTIM to allow for obstacle avoidance. The 
following results using program options 533 points out the 


importance of choosing a correct FINTIM. As can be seen in 


Zo 


Figure 5.1, FINTIM can adversely affect the problem 
solution if the time allotted is not large enough to 
achieve the desired result. When FINTIM is chosen to be 
6.0 non-dimensional time units (NTU), the avoidance zone 
constraint for zone 3 is violated and the desired depth of 
17.425 feet is exceeded. When FINTIM is increased to 7.0 
NTU, avoidance zone constraints are violated for zone 1 and 
zone 3 and the desired depth is not achieved. However, the 
severity of the violations are not as blatant. When FINTIM 
1s increased to 8.0 NTU, the desired problem solution is 
obtained. Table 3 presents the computational costs 
associated with each FINTIM selection. Note that the 
optimization problem is easier with more maneuvering time, 


therefore the computational cost is less. 


TABLE 3. FINTIM COMPUTATIONAL COST 


FINTIM TIME (sec) 
6.0 95.06 
7.0 76.75 
8.0 32.43 
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VI. PROGRAMMING FOR THREE DIMENSIONS 


In programming for three dimensions, we not only 
optimize the problem to obtain bow plane and stern plane 
commands, but we also optimize to obtain the rudder 
commands. In order to achieve the desired result, it was 
necessary to increase the number of design variables for 
the rudder in the linear model. The problems discussed 
previously, have all been solved using ten discretizations 
(design variables) for the stern plane and bow plane 
inputs. In the three-dimensional work, the number of 
design variables were arbitrarily increased to twenty 
(less discretizations would not achieve a satisfactory 


result). 


A. SIDE CONSTRAINTS 

Additional constraints were added to the problem in 
order to ensure reasonable vehicle control surface 
reactions. The maximum rudder angles were set at plus or 
minus thirty degrees. In order to ensure this, the side 
constraint approach was invoked. These values were 
assigned to the Design Variable Lower Bound (VLB) and the 


Design Variable Upper Bound (VUB) ADS parameters. 
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B. EQUALITY CONSTRAINTS 
Six additional equality constraints were needed to 
achieve the desired y position and the desired vehicle 


condition (yaw and roll) at the desired end condition. 


C. CONSTRAINT SCALING 

Sanders [Ref. 19] points out that constraint weighting 
is important in achieving the desired results. This is 
even more crucial in a three-dimensional problem solution 
because of the increased number of constraints on yaw, 
roll, rudder control and y positioning. It appears that 
problem sensitivity to constraint weighting is also 
increased. In order to achieve the desired solution 
result, it was necessary to adjust constraint scaling 
factors until all constraint conditions were satisfactorily 
obtained. Table 4 shows the constraint scaling factors 


used in the full three-dimensional linear model. 


TABLE 4. CONSTRAINT SCALING FACTORS 


CONSTRAINT SCALING FACTOR 
depth Oso 
Vy pos ition 25 
pitenh 1.0 
yaw a0 
roll 1516 
minimum depth 18 
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D. LINEAR/NONLINEAR DYNAMICS 

Figure 6.1 illustrates the nonlinear model behavior 
when using control inputs for bow plane, stern plane, and 
rudder from the optimized linear model. It is evident that 
these commands are invalid for the full scale nonlinear 
model since the final objective state is not closely met. 
Therefore, the essential dynamics of the linear model are 
not valid in three dimensions as might be suggested from 
the results of the previous study. However, even though 
the control surface inputs are invalid, the vehicle state 
trajectory is valid because the path chosen achieved the 
desired result. For a desired position of y=40.0 feet and 
depth =-20.0 feet, the obtained result was y=40.269 feet 
with a depth of -20.699 feet. These values can be fine 
tuned by varying the scaling factors. Figures 6.2 and 6.3 
illustrate the control inputs to the linear model and 
Figure 6.4 illustrates the linear model response to those 
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E. THREE-DIMENSIONAL COMPUTATIONAL COSTS 
Table 5 compares the virtual machine time of the full 
scale linear model with no obstacles as compared to the 


full scale nonlinear model with no obstacles. 


TABLE 5. LINEAR VS. NONLINEAR COMPUTATIONAL COSTS 


LINEAR NONLINEAR 

(sec) (sec) 
DIVE PLANE ONLY PAN 2S 108.81 
FULL 3D 130.34 370761 
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VII. VALIDATION RUNS 

As previously mentioned, in order to validate the 
selected optimization configuration, fixed obstacles were 
placed at various positions in the AUVs field of view with 
1.0 foot radius avoidance zones around the obstacles. 
Figure 7.1 to 7.8 illustrate the paths chosen by the 311 
algorithm to avoid the obstacles and their avoidance zones 
for various obstacle positions. 

The moving obstacles were simulated using rectilinear 
average velocity equations of the form: 

s = Vt 

where: 

S = position of obstacle (X and/or Y) 

V = constant velocity 


t = time of travel 


Figures 7.9 to 7.11 present the distance between the AUV 
and the moving obstacle(s) as a function of time. As 
seen, the algorithm chooses a path in both cases which 
avoids impact. Table 6 presents the computational cost 
comparison of various obstacle case(s). Some three- 
dimensional linear and non-linear state trajectory results 


were presented earlier. 
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Figure 7.10 


TABLE 6. PROGRAM 311 COMPUTATIONAL COST-2D 


NUMBER OF OBSTACLES 
0 
1 (moving) 
2 (moving) 


7 CE yxed } 
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TIME (sec) 
14.23 
23.688 
24.68 


S752 


VIII. CONCLUSIONS AND RECOMMENDATIONS 


A. CONCLUSIONS 

The following conclusions can be drawn from this 
feasibility study: 

1. Optimal control theory is a feasible method for 
determining obstacle avoidance paths in the presence of 
fixed and moving obstacles. 

2. The introduction of obstacle constraints into the 
algorithm increases the computer computational costs. 

3. The full linear model control inputs are not 
compatible with the full nonlinear model. When linear 
commands were placed in the nonlinear model, the vehicle’s 
end condition was inconsistent with the desired end 
fonaition. 

4. The best general algorithm for the two dimensional 
case was determined based on the ability of the algorithm 
to solve a variety of obstacle problems with a shortened 
EENTIM. 

5. Scaling factors can be critical in achieving a 
desired problem solution. 

6. In order to achieve a solution in three 
dimensions, it is necessary to increase the number of 


design variables for certain vehicle control inputs. 


4? 


7. #FINTIM 1s a critical variable whose value can 


change the solution to a specific problem. 


B. RECOMMENDATIONS 


1. Find a procedure to estimate the optimal scaling 
factors for end constraints. 


2. Study the discretization factors in the three- 
dimensional case(s). 


3. Study the selection of FINTIM in the two-dimensional 
case and in the three-dimensional case. Vehicle 
mission objectives should be considered in 
establishing guidelines. 


4. Develop the optimization of the three-dimensional 
nonlinear model. 


5. Develop the algorithm to include optimization of 
the propeller rpm control input. 


6. Develop the program for efficient programming in a 
microprocessor. 
7. Determine if the general algorithm recommended in 


this thesis is also applicable for three-dimensional 
obstacle avoidance cases. 
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APPENDIX 


PROGRAMS 


This appendix contains the four primary programs that 


were used for this feasibility study. They were: 


ie 


OBST DSL - This is the state linear 2D model used 
for the 2D analysis. 


TLO DSL - This is the ADSL program used to optimize 
the full scale linear model of bow plane, stern 
plane and rudder control vectors. 


TNLO DSL - This is the ADSL program used to optimize 
the full scale nonlinear model for timing comparison 
studies. 


TLNLO DSL - This is the ADSL program used to 
optimize the full scale linear model for bow plane, 
stern plane and rudder control inputs and with 
Simulation of the full scale nonlinear model. 
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CZb Us—Z0 bond) xCZrUs—-Z0BS10'> >} 

Dea webeouRiCCxr OS-XOBS]lOJACXPOS-XOBSI0)+. 

CZ, 02-ZUBs Gy xCZPO0S-Z0B510>) 

Dany eG =oOhi CCXPOS-XOBS10)*%(XPOS=XOBS10)+. 

CAnUa=2uUb>! 0) xXCZP0s-Z05510) > 

BoeveoeooniCC Ar OS—-X05510)x(XPOS-AOBSI0)+. 

CZF 0s-Z0Bs10)*x(ZP0S—-Z0BS10)) 

DoAvVeGesth tT CCArP Gs >XOBSIOACXRPOUS-AOBS10O)+... 

C2ZnUSecobS lO) AC ZP05-Z0BS109> 

Devers oOo OR NCCxRO oe, OUBSIO)*CXPOS-XOBS10)+... 

Cente aZ70bs10) x(ZPOS-Z05510)) 

Dom ecocou ni CAr Os=xXOB S10) x(XPOS=xX0BS10)+.... 

CZs seo bstUO x (ZPO0S—-Z0B510') >} 

DSAVEH=SORTCCXPOS-=XOBS10)x( XPOS—-XOBS103+. 

CZEUS=SZ0535 10) xCZP0S-Z0BS10)} 

ORDBDER = — 1.0 

Boa ZN 

A2=(MA-ZWDOT ) 

A3=-CZQ+MA) 

AG=-ZQDOT 

Bl =-MW 

BZ=—-MiDOT 

B3=-MQ 

BG=CIY-MQDOT ) 

BS=-MTHETA 

Ci-Z)s 

C2=ZDB 

C3=MDS 

C4=MDB 

Ca-CS=Cb4xC1)7A4 

C6=C4G-(CB4XC2)7AG 

D1=B2-CB4XA2)7A4 

D2=B1l-CBSXA1I/AG 

D3=B3-(B4XA3)7AG 

CALL DADSCINFO,ISTRAT,IOPT,IONED, IPRINT, IGRAD,NDV,NCON,DX,... 
VLB,VUB,OBJ,GWN,IDG,NGT,IC,DF,AW,NRA,NCOLA,WK,NRWK,.. 
IWK, NRIWK) 

PeculEe: £Q00) THN 

i ee age DEL TH, PITCH, DS, DB, BOWANG, STNANG 

Sie 

De 

Te NEO. EQ. 0) sDELPRT 

Pe CINE EQ: 0) DELPLT 


oO 
NOD 


noo 


DERIVATIVE 
% 


a1 


THETDD=(1/7AG)*(C1*DS+C2XDB-AlXW-A2XWDOT-A3XTHETAD) 

WDOT= CI1/D1)¥(CS5XDS + CéxDB = D2ZXN = DSXTHETAD [eb S* teem 

TRETAD= THTGRLCTHETAO, THETDD) 

TRETA = IHTGRLECTHETAO, THETA DD 

Ho= INTGRL CHO, WDOT ) 

Z = INVGRLCZOSH=U0AT bie 

Der h= 4 

PITCH=THETA7.01745 

BOWANG=C(DB/7 .01745) 

STNARG=CDS7.01745) 

THTGRD = CCNXNt( Z-ORDDEP )XCZ-ORDD ER 
THETADXTHETAD+THRETAXTHETA)) + (DS*¥DS+DBXDB) 

OBJ1 = INTGRLCO.,(€0.5)XINTGRD) 

OBJ = OBJ1 


x 
DYNAMIC 


KK MK OK KK MK 


KK K 


RN=TIMEZCEINTINZ 10  —DELI7ZLoutG 
O=INTCRND+1 
if COmeO cL Tmo =19 


ADDITIONALLY THE PLANES SHOULD BE AT EQUILIBRIUM SO THE 
VEHICLE WILL PROCEED AT THIS NEW DEPTH ATA oo ee Oeen. ee 


DS=DX(0O) 
DB=DX(10+0) 
1FCO.CE1005 23528. 
TF<O.GE. 10)" Dae OF 


CONSTRAINTS FOR A DIVE 


ORDERED DEPTH = s0kD0ER 


GHiC1l) -=CZ=-ORDDERIZz2. 
GWC2) =CORDDEP-2Z)72 


AUV'S FINAL STATE MUST BE LEVEL FLIGHT AS FOLLOWS 


GHC3)d = THETA X10. 
GINNC4)' = =THETAXIC. 
GHC5)==Z221060- 


X=Z POSITIONING FOR OBSTACLE AVOIDANCE 


AROS =27 425% ie 
ZPUS==Z2 47 425 
DT=TIMNEXZO./7FINTIM 


U1 
NI 


~ AVOTOLEG THe OBSTACLE 

Pie se OOXE Oo. Oba) ) XOX OS—-X0BS1)+(ZP0S-Z0BS1)*%(ZPOS-ZOBS1)) 
Dao ee oe 2 x OXe > -X0BS2)4+( ZPOS-Z0BS2)x(ZPOS-ZOBS2)) 
Dee = Ge Oo boo x (Xo X0RS57+(ZP05-Z0BS3)x( ZPOS-Z0BS3) ) 
Diste-o0r WCt XPOSSX0BSS) x(XFP0S-XOBS4)+(ZPOS-Z0BS4)xCZPOS-Z0BS4 ) ) 
Peso — son AP OS] x 0b >> xX(XPUs—-7,0BS5)+( ZPO0S-ZOBS5)xX(ZPOS-ZOBS5) ) 
Diese - Cn eux Osx 0noo) xX (XPUs-x0n56 )+(ZP0S—Z0BS6 )x( ZPOS-ZOBS6)) 
Dio =e PC AR os-AUbs7 JX (XPUssx0E>7 )+(ZP0S—-Z0BS7 ) x (ZPOS-ZOBS? )) 
Dies Oe Cr Oo Uboo eC xP0>- 0Ess)+(ZP0S5-Z0B5e)x(ZPOS-Z0BSS) ) 
Die on XP 0s—K0 boo xX 05.7 0b 9) 4+CZP0S-Z0BS9)*(ZPOS-ZOBS9) ) 
Pe uO esor CC xr Os=x0no LUI ACK Use Obs LOVECZPOS-ZOBS10)x.. 


OZAr0S=20b510)) 

Distt oun CCxrOSssx0bst lx XPUSe OB otl+CZPOS-ZOBSI1)X.. 
C27 05-Z0B511)9 
Bee Senex Os-XOBS1cCyx( XPOS] KOEI CEC ZPOS-=ZOBSI12)%... 
CZRUS=Z0B51c)) 
Dol OR xr Oss xX 0BS1S) xX( XP 0S sx0BS13)+(ZPOS-ZO0BS13)x... 
UZFUSsZ0B513).) 
Dio Gs ek CCxXF OSs-A0RS1 4) xCXFO0S>=x%0BS 14) 4( ZP0S-ZOBS14)x. 
CZEDS-Z0B514)) 

De ioe en) (OC XP OS-XOBS1 5) XC XP USexX0bS 5) 4+°CZPOS-ZOBS15)x... 
C250s—2068515) ) 

Dios = Shi CXROS-XO0BS16)X( XPUS-xXOBS1G)t(Z2P0S-ZOBS16)xX... 
CZ oaZUBS16 ))) 

eee Sen CCAT US -XOBGl/ )XCXEUS-7URS27 JACZPOS-ZOBS17)%.. 
eZEUS=20B51/)) 

Neo omlaLteDoAVEl) DSAVEL=DIsthi 

ReGetole- Lie UsAVEc) DSAVEZ=DIST2 

PeColoio. LV DSAVvES) DSAVES=DISTS 

IFCDIST4.LT.DSAVEG) DSAVEG=DIST4 

heels hS.LT.DSAVE5) DSAVES=DIST5 

IFCDIST6.LT.DSAVE6) DSAVE6=DIST6 

mecOlei7 Ei DSAVE?7 )«DSAVE7=DIST7 

PeeDisto ..L).DSAVES) DSAVES=DISTS 

PaCUlsT 97 Ei. DSAVES) DSAVE9=DIST9 

IFCDIST1O.LT.DSAVEA) DSAVEA=DIST10 

Peewee sDonvEB) DSAVEB-DIST11 

PeGubotlcob)-DSAVEC) DSAVEC=DIST12 

IFCDIST13.LT.DSAVED) DSAVED=DIST13 

IFCDIST14.LT.DSAVEE) DSAVEE=DIST14 

NaGutollo ob DoAVEr) DSAVEF=DIST]5 

IFCDIST16.LT.DSAVEG) DSAVEG=DIST16 

Peet St ly tere DSAVER) DSAVER=DIST17 


Ghigo? -(9.-NonvEl 
GWC7)=C5.-DSAVE2) 


GHGS =X S.>DOAVES) 
GHC9)=C6.-DSAVES ) 
GliC10)=C1.-DSAVES5) 
GWC11)=C1.-DSAVE6) 
GHC12)=C1.-DSAVE7) 


ebay “[disys DSL 
GiiCls)=CiPSpoAVine® 
GHC14)=C€1.-DSAVE9) 
GHCTS = Ole pen ven? 
GWC16)=C1.-DSAVEB) 
GWC17)3=¢€1.-DSAVEC) 
GHC183=C1.-DSAVED) 
GHCT9) =i. -DsAVEes 
GHC 20)=¢C1.-DSAVEF) 
GHC21)=Cle-DSAVEG? 
GNCZ2)=( Tee DSAVEn) 

TERMINAL 


END 
S101 


BRCINPO VEO 40) iin 


PRINTX, DSAVE1, DSAVE2 
PRINT®, DSAVES, DSAVES 
PRINTX, DSAVES, DSAVE6 
PRINTX, DSAVE7, DSAVES 
PRINTX, DSAVE9, DSAVEA 
PRINT®,DSAVEB, DSAVEC 
PRINTX, DSAVED, DSAVEE 
PRINT®, DSAVEF, DSAVEG 


PRINTX, DSAVEH 
EESE 
ENDIF 


IFCINEFO. EQ. 0) “CARES END Os 


CALL RERUN 


94 


ELLE: 1TLO BSE Al 


Mriee RUN; 16-5 LINEAR AUV MODEL 7 STERN PLANE AND BOW PLANE SEPARATED 
x (1) UPDATED:049/716788 

Pee el O00 Fr DEPTH CHANGE IH’ 20 SEC 

* (3) RIGHT OBJ EQUATION 

* €4) ADS CONSTRAINTS ON DEPTH AND PITCH 

* (5) OBSTACLE FURTHER DOWN THE TRAJECTORY AND ABOVE IT 

* (6) CORRECT OBSTACLE AVOIDANCE ROUTINE ADDED 


KKM KK KKK KK KKKKKKXKXXXKXADSL SET-UP XK KKK KK KKH HKK HH KKH KK KKK KKK KKK KKK 
eee istkal, Lori, LIONED, IPRINT, INFO, IGRAD, NDV, NCON 

FIXED IDG, NGT, IC, NRA, NCOLA, NRWK, IWK, NRIWK, O, H,D,C,P 

D DIMENSION AWCG42,492) 

ARRAY WKCG000), IWKC1000) 

pier DXCG0), VLBCG0), VUBCG0), GWNC11), DFCG1), IDGC11), ICC11) 
PARAM NRA=42, NCOLA=42, NRWK=4000, NRIWK=1000 

PARAM IGRAD=0, INFO=0, NDV=G0, NCON=l1, NGT=11 

feoniee DXCIl—2)=2% .0,DX03-21)=19%0., IDGC1-10)=10x-1 

TABLE DX€22-G0)=19xX0. 

iapeLeE— IDG(7-0)=1*1 

feel VL BCl-9)=9x—-.1749652, VLBCLI-19)=09%—.29493,VLBC10)=0.,VLBC20)=0. 
Mepee VUBC1—-9)=9% 174652, VUBC11-19)=9%.2943,VUBC10)=0.,VUBC20)=0. 
peepee VE BC 21-59) =19x— .523627 , VUBC21-39) =19%.523627,VUBCG0-G1)=0. 
TABLE VLBCG0-G])=0. 

PARAM ISTRAT=3, IOPT=l1, IONED=1, IPRINT=0000 

INCON H=0, OBS1=0.,YZONE=0. 

HetmOD RECT 

SomrROL FIHTIM=21. , DELT=.10 

eee XP OSM, YPOsM, DEF TH, THETAM,PHIM,PSIM, DSM, DBM, DRM 

Bei! XPOSM,YPOSM, DEPTH 

Sree? Sit, DBM, DRM,PITCHM, XPOSM, YPOSM, DEPTH,NDT 

peel tHETAD,W, DEPTH,PITCH, XPOS, DEPTH,NDX,NDZ,NDT 

Peve ITHETA,W,Z,DEPTH,PIICH,DS,DB, BOWANG,STNANG 
XRAPHCDE=TEK618) TIME,DS 

*RAPHCDE=TEK618) TIME,DEPTH 

XRAPHCDE=TEK618) TIME,WDOT 

XRAPHCDE=TEK618) TIME,W 

XRAPHCDE=TEK618) TIME, THETDD 

meaenc DE=TEK618) TIME, THETAD 

ereern«DE=TEK618) TIME,THETA 

Pear DE=TEK618) TIME,PITCH 

XRAPHCDE=TEK618) TIME, BOWANG 

freer mcDE=TEK618) TIME,STNANG 

KKK KKKKXKKKXKXXRXKXXXXXXD S L MODEL FOR LINEAR SIMULATION X*XXXXX¥XXXRKRKXKKXKKKKX 


x LINEAR MODEL ONLY 

D COMMON’7BLOCK17 MMINV(6,6), MMC6,6), AAC12,12), BBC6,6) 

D COMMON/BLOCK2/7 BC6,6),AC12,12), UMODC6),GKK(6,21) 

D COMMON’ BLOCK3/7 FC12), FPC6), UCFCG) 

D COMMON’ BLOCKG/ G64(04),GKG(09),BRC49),HHCG) 

D COMMON/BLOCK57 XDOTC12),XDOTXC12), XDOTUC6) 

FIXED N,IA,IDGT,IER,LAST,J,K,M,JJ,KK,1 

INTEGER 

ARRAY WKAREAC54), X12) 

x 

x 

SOS | 

Xx 

x LONGITUDINAL HYDRODYNAMIC COEFFICIENTS 

* 

Bete xre = 7.03E-3 ,xXQQ = -1.47E-2 ,XRR = 4.01E-3 ,XPR = 7.64E-G,.. 
UU On eote AW =" >legce-) >AVP = =3.29E-3 -XVR = 1.89E~2,... 
mbes manoWe-2 6, XUDR= —2.6E-3 Pn UReme Oe horaG es, AVY = 5.29E-c,. 5. 
Bose ete ol p> xXAVDR= 1.73E-3 »XNDS= 4.6E-2 PAD E= 9,66 E= 57454 
eo eet Cee ee wUDUb =o 07 Eso) »ADRDR--1.01E-2 ,XQDSN=1.96E-3,... 


MADEII=S.96E=5 ,XDSDSN=-1.62E-3 
2 


x 
CORT 
COWST 


LATERAL HYDRODYNAMIC COEFFICIENTS 


YPDOT=1.27E-4 {YRDOI-1).2%e,4 »YPQ = 4.125E-3 ,YQR =-6. 5G se 
YVDOT=-5.55E-2 ,YP = 3.05S5E-3 (Re eee ,»YVQ = 2.360523 
YHP = 2.35E-1 ,YNR = -1.8€E-2 ,YV = -9.31E-2 ,YVW = (600 ce 
VOR SS 36-1 SE>Z yy CDN eS eel 

NORMAL HYDRODYNAMIC COEFFICIENTS 

2QD01=-6.61E-35 72h he=nl cee »ZPR = 6.67E-3 ,ZRR =-7 .35Beee 
ZWDOT=—2.95E>1 , 200s ,ZVP = -G.81E-2 ,ZVR = 4. 55 Scee 
ZW = -3.02E-1 ,2ZVV = -6.84E-2 ,ZD5 = -7.255E-2,ZDB --2) 560 
ZQH = -2.88E-3 ,ZWN = -5.07E-3 ,{ZDSN= =) 0NSE-2,002- See 


ROLL HYDRODYNAMIC COEFFITIENTS 


KPDOT=-1.01E-3 , KRDOT=-3.37E-5 ,KPQ = -6.93E-5 ,KOQR = 1] (Gg 
-8.41E-4 ,KVQ=—-5. )PSee ee 
3.055E-3 ,KVW =-] (187 


KVDOT=1.276-9% oF Ge ee »KR 
KHP = =-1.27E8=46 4 (KlIR Gl of a7 aaa 
KPH = =5./3E=9) KDB @-=SenGe—3 


PITCH BY DRODYHAMIC COERFICIENT. 


MQDOT= ~1.68E-2, MPP = 5.26E-5 ,MPR = 5.0GE-3 ,MRR =-2 006 Re 
MNDOT= -6.81E-3, MQ = -6.86E-2 ,MVP = 1.18E-3 ,MVR = 177 SE 
Mio = 9.86E-2 » MVV = -2.51E-2 ,MDS = -4.12E-2 ,MDB = 67> 9@ Re 
iQN = -1 646-3 , MAN = =—2.6SE-3 | NUS S-ao-s Claas 

YAW HYDRODYNAMIC COEFFICIENTS 

NPDOT=-3.37E-5 , NRDOT=-3.4E-3 ,NPQ = -2.J1E-2 ,NQR = 20750 
HVDOT=1.24E-3 , NP = -8.4605E-4 ,NR = -].64E-2 ,NVQ =-9 (90 Eee 
NWP = -1.75E-2 , NWR = 7.35E-3 NV = -7.42E-3 ,NVW =-2767 Eee 


WORK (= +1. 296-2 
MASS CHARACTERISTICS OF THE FLOODED MARK IX VEHICLE 


WEIGHT = 15900 , BOY = 15900 »VOL = 248.494 ,XG =-078 
YG = 0.0 » 2G = 0.061 »XB =-0.1 ,2B =0.023 
IX = 1760 » IY = 9450 »1Z = 10700 »IXZ = S65iGe 
1 6 See pe Sr = dee »¥YB = 0.0 

L = 17.425 » RHO = 1.94 ,G = 32.2 »NU = J > Sia 
RO) = leo juste Ue = 10220 »NPROP = 0.0, X1ITEST=0.01 
DEGRUD= 10.0 »DEGSTN= 0.0 

X0B31=36.0 


COGS T= eae 


Ul 
J) 


x 
x 


INPUT INITIAL CONDITIONS HERE IF REQUIRED 


TN Tae 


x 
x 
x 


x 


NOSORT 


KK KN 


DoavEl=s0R)(CXPOSM-XOBSIIXCXPOSM=XOBS1)+... 
CZEUSM=Z0BS)> XCZPOSM-=Z0B51)) 
PNITTALITZE ALL MATRICES AND ARRAYS 10 ZERO 


ORDDEP=20. 

YORD= GO. 

D=0 

H=H+] 

IF CH.EQ.1) THEN 

N = 6 

DO 2 J =1,N 
JJ= J+N 
DO 1K & 
KK= K+ 
KKK= KK 
MMINVCOJ, 
XCJ) = 0 


yN 


GKKCJ,KK 
CONTINUE 
CONTINUE 


INPUT THE LINEARIZATION POINT PARAMETERS 


On 
So 


V0 


me 
© 
Le | 
- OoOtonocaoonTe& 


7 


KKK KK OK OK 


K OK 


KK OK OK 


*K 


KK OK OK OK OK OK OK OK OK 
a IE ee | 


INPUT THE MODEL STATES INIVTIAES CEE aos 


UM = 6.0 

VM = 0.0 

WM = 0.0 

Pit = =O30 

QM = 0.0 

RM = 0.0 
XPOS = 050 
YPOSM = 0.0 
ZPOUSM =~ 020 
PHIM. = 0.90 
THEEAM. =..00 0 
P Sih = 30.0 


INPUT THE VEHICLE INITIAE CONSTI Ton. 


THITIAGIZE THe CONT ees 


DBOY= 0.0 
DR=0.0 

DS= 0.00000 
Doria Or 
DBM=0. 
DB=0.000000 
DRM=0.0 
DRPM=0. 

RPM = 500.00 
LATYAW = 


= 0.0 
NORPIT = 0.0 


MASS = WEIGHT/G 


DIVAMP = DEGSTN¥0.0174532925 
RUDAMP = DEGRUDx*0.0174532925 


THE LINEAR PROPULSION MODEL 


ETA 0.012*RPM/U0 
ETA 0 
RE = UVOXL/ZNU 


COO = .00385 + €1.296E-17 XC KE ea 
CT = 0.008*LXX2XETAXABSCETA)7(CAD) 
CT1l = 0.008XL®xX2/CA0) 
FPS = -1.04+CSQRTICCT+4+1.0)-1.0)7CSQRIC CTs 1p ie 
XPROP = CDOXCETAXABSCETA) =e 
N=6 
DO 15 J = 1,N 
DO 10 K =1,N 
MMINVOJS,KI=0.0 
MMCJ,K) = 0.0 
CONTINUE 
CONTINUE 


CALCULATE THE MASS MATRIX 


MMC1,1) = MASS -CCRHO/2) CL 3) XXUDOT) 
MMC1,5) = MASS®ZG 
MM C1516) “== AS Sx vo 


mK 


KK KK OK 


hinoene De Hj ablnoo —- 6 RHOZ 2 xe xxS)xYVDOT) 
MMC02,49) = -MASS¥ZG -CCRHO/2)*X(L¥XSG)*YPDOT ) 
MMC2,6) = MASSXXG - CCRHO/2)*(LXX¥G)X*YRDOT ) 
hin Gees — aos — CCRHO72)xCELxxX3)*ZNDOT D 
MMC3,49) = MASSXYG 
MMC3,5) = -MASS*®XG -CCRHO/2)*C(L¥*G)*ZQDOT) 
Pease = =MASS*ZG — COCRHO7 2) x(LXxaDxXKVDOT) 
MMCG,3) = MASS*YG 
MMCG,49) = IX - CCRHO/2)*C(L®X5)XKPDOT) 
PMcg, 5) = —1TXY 
pie 456) = ~I1XZ —-( CRHOZ 2) XC LXX5)X*XKRDOT) 
MMC5,1) = MASS*ZG 
MMC5,3) = -MASS®XXG -CCRHO/2)*( LXX4)XMWDOT ) 
MMC 5,6) = —EXyY 
ico Yom RHOA 2 XCEL xx 5) xMODOT) 
Plies poorer =— — 112 
MMC6,1) = -MASS*YG 
MMC6,2) = MASS¥XG -CCRHO/2)*C(L¥*¥G)*NVDOT) 
MMC6;9) = -IXZ = (CCRHO/2)*¥(LXX5)XNPDOT) 
Pico aes ee 

6,6) = 


V2 eC CORH O72 Ix CExx5 ) XNRDOID 


LAST = NX¥N+3XN 
DO 20 Vise LAST 
WKAREACM) = 0.0 
CONTINUE 


IER = 0 
IA = 6 
IDGT = 4 


CALL LINV2FCMM,N,IA,MMINV, IDGT,WKAREA, IER) 


CALCULATE THE A MATRIX FOR THE LINEAR MODEL 


AC1l,1) = RHO/2XL¥X3XCXQDSXDSXQO+XQDB/2XDBXQO+XRDRXROXDRI+... 
RHO/2XL ¥X2*XCXVDRXVOXDR+XWDSXDSXWO+XWDB/2XDBXWNO + . 
2XUOXCXDSDSXDSXX2 + XDBDB/2XDBXX2 + XDRDRXDRXX2))+ ... 
RHO/2XLXX3XXQDSNXQOXDSXEPS+RHO/ 2XL XX 2X CXWDSNXWOXDS+... 


2XXDSDSNXUOXDS XX2) XEPS+tRHOXL XX2XUOXXPROP+RHO/ 2XL XXSX... 


XQDB/2XDBXQO+RHO/ 2XL XX 2XXWDB/ 2X DBXWO+RHOXL XX2XU0X... 
XDBDB/2XDBxXX2 


FIVE: 


eae) 


AC@1,2) 
ACs) 
AC1,49) 
AC@1,5) 


AC@1,6) 


AQ1,11)5 


AC2,1) 
ACZy 2) 
AC2,3) 
AC2,49) 
AC2,5) 


AGZ 76) 


AC2,10) 
ACZ, 11) 


AC3,1) 


ACG,2) 
AC4, 3) 
AC4,49) 
AC4,5) 
ACG,6) 


Te 


AC4,10)= 


ACG,11)= 


DSL Al 


MASS¥RO+RHO/2*L XXSXCXVPXPO+ XVRXRO) + RHO/2XL*XX2*X 
C2*XXVVXVO + XVDRXUOXDR) 

“~MASS*QO + RHO/2XLXX3XCXWQXQ0 )D+RHO/2XLXX2KC2]XXWWXWO+... 
XWDS¥DSXU0+ CXHDB/2*XDB+XWDB/2XDB)XU0 +XWDSNXUOXDSXEPS) 
-MASS*YGXQO-MASSXZGRRO+ RHO/2XLXXGXC2*XXPPXPO+XPRXRO)... 
+ RHO/2XL XX3XCXVPXVO ) 

—~MASS*XWO+2*XMASS*¥XGKQO -MASSXYGXPO+RHO/2ZXL XXGX2XXQQKQO... 
+RHO/2XL XX3*CXWQKHO+XQDSXDSXU0+XQDB/ 2XDBXUO )+RHO/ 2X 
L¥*X3*XQDSNXUOXDSXEPS+RHO/2XL XX3XXQDB/2XDBXUO 
MASS*XVO+2*XMASSXXGXRO-MASS*XZGXPO+RHO/ 2XL XXGRXC2*XXRRERO... 
+ XPRXPO) + RHO/2XL¥X3XCXVRXVO + XRDRXUOXDR) 

-CWEIGHT - BOY) XCOSCTHETAO) 


~MASSXRO+RHO72XL XXSXCYPXPO+YRXROD+RHO/2KLXX2KXCYVXVOtT... 
2*YDRXUOXDR ) 

RHO/2XL XX 3XYVQXQO+RHO/2XL XX2*XC YVXUO+Y VW XID ) 

MASSXPO+ RHO/2*XLXX3XCYWPXPO+YWRXRO )+RHO/2XL XX2KYVWXVO 
MASSXWO-MASSXXGXQO0+2XMASSXYGRPO+RHO/2XL XXGXYPQXQO+... 
RHO/2XL¥X3XCYPXUO+ YWPXWO) 

—-MASS*XGXPO-MASSXZGRRO+RHO/ 2XLXXGRCYPQXPOFYQRXRO) +... 
RHO/2*L XX SXYVQXVO 

—MASSXUO0+2*XMASSXYGRRO-MASS XZGXQO+RHO/2XLXXGXYQRXQO +... 
RHO/72XLXX3XCYRXUO + YWRXWO) 

CWEIGHT - BOY) XCOSCTHETA0)XCOSCPHIO) 

-(WEIGHT - BOY) XSINCTRETASDXSTNGCPH ne 


MASS*QO+RHO/2XL XX3XZQ¥KQO+RHOY 2¥L XX2*C ZWKXWO+2XU0XZDSXDS... 


+2KXUD0XZDB/2XDB+C ZWNXHO+2XZDSNXUOXDS )XEPS D+RHO/2XLRXSX... 
ZQONXQOXEPS+ RHO/2XL XX¥2X2XU0XZDB/2XDB 
-MASS*PO+RHO/2XLXX3XCZVPXPO+ZVRXRO)D+RHO/ 2¥XL XX2KX2KZVVRVO 
RHO/2XLXX2*XCZWKUO + ZHINXUOXEPS) 
—-MASSXVO-MASSXXGXRO+2*XMASSXZGXPO+ RHO/2XLXXGXC2*XZPPX.. 
PO + ZPRXRO)D + RHO/2XLXX3XZVPXVO 

MASSXU0 - MASSXYGXRO+2X*MASSXZGXQO+RHO/2XL¥X3XZQKUN +... 
RHO/2XL XX 3XZONKXUOXEPS 
-~MASSXXGXPO-MASS XYGXQO0+RHO/2XLXXGXCZPRXPO+2XZRRXROD+... 
RHO/2XL XX 3XZVRXVO 

=C(HEIGHT = BOY) *xCOSCTHETADD XS TiGCERnEUD 

-CWEIGHT - BOY)XSINCTHETAO)XCOSCPHIO) 


MASS*YGXQO0 + MASSXZGXRO + RHO/2*LXXGXCKPXPO + ... 
KR¥RO)+RHO/2XLXX3*XCKVXVO+Z2KUOXC KDB/2XDB-KDB/2XDBI D+... 
RHO/2XL¥X3XUOXKPROP+ RHO/2XL XXGXKPNXPOXEPS 

-MASSXYGXPO + RHO/2XL¥XXGXKVQXQO0 + RHO/2XLXX2xXCKVXUO0... 
+ KVWXHO) 

-MASS*XZGXPO + RHO/2XL*XXGXCKWPXPO + KWRXRO) + 
RHO/2*L XX 3XKVWXVO 

-IXY¥RO + IXZ*Q0 - MASSXYGXVO - MASSXZGXHO + 
RHO/2XLXX5XKPQXQ0 + RHO/2XL*XX4GXCKPXUO+KWPXWO ) 

-IZXRO + ITY*¥RO + 2*XIYZXQO + IXZXPO + MASS*1G*UUR Ie 
RHO/2*XLXX5XCKPQXPO + KQRXRO)D + RHO/2XLXXGXKVQXVO 
—-IZxQ0 + ITYxXQO - 2X1 YZXROU + HASSAZO Ue 
RHO/2XLXX5XKQRXQO + BOO ee Cana enn ead 
~CYGXHEIGHT-YBXBOY )*XCOS( THETAQ XS URE ree 
-(ZGXWEIGHT-ZBXBOY )XCOS( THETAQ)=Ce sarin 
-(CYGXWEIGHT-YBXBOY)*SINCTHETAO)XCOSCPHIO). 

+( ZGXWEIGHT-ZBXBOY)XSINCTHETA0) XSINCPHIO) 
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AC5,3) 
AC5,4) 


AC5,5) 
AC5,6) 
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A(6,1) 
AC6,2) 
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AC6,49) 
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AC8,11)= 
AC8,12)= 


mom Wow ow 


—MASS*XGXQO + RHO/2XL¥XGXMQXQO + RHO/2XLXXSXMNXWO +... 
RHO/Z2XL¥XSXUOXCMDSXDS+MDB/2*XDB) + RHO/SZXLX*XGXMQNXQOX... 
ote RMO7ZAP A oxi en + 2AMDSHAUOXDS)XEPS+... 
RHO/2XL XX 3XU0XMDB/2XDB 

MASS¥XGXPO + MASSXZGXRO + RHO/2XLXXGXCMVPXPO + 

MVR¥RO) + RHOXLXX3*XMVVXVO 

-MASS¥ZGXQ0 + RHO/2XLXXSXMNXUO + RHO/2XLXX3¥MNNXUOXEPS 
gi .4n0 +el2eROe- IyZQmeecxIXZxPO + MASS*XXGXVO + ‘ 
RHO/2XL¥X5XC2*XMPPXPO + MPRXRO)D + RHO/2XL¥XGXMVPXVO 
IXYXRO -IYZ*PO - MASSX®XGXUO -MASS*®ZGXWO + RHO/2X... 
L¥X¥GXMQXUO + RHO/Z2XLXXGXMQNXUOXEPS 

BUAAEO + IZXP OR xy xGU 2-1 xXZxRO + MASSXZGXVO +... 
RHO/2XLX¥X5XCMPRXPO+2XMRR¥ROIV+RHO/ ZL XXGREMVRXVO 
(XGXWEIGHT-XBXBOY) XCOSCTHETA0)XSINCPHIO) 


(XGXHEIGHT-XBXBOY)XSINCTHETAO)XCOSCPHIO) - 
(ZGXWEIGHT-ZBXBOY)*COSCTHETAO) 


—MaooxxGeRO + RHO/ZXLXXAGXCNEXPO +NRXRO)D + RHO/2X... 
L¥*¥3XC(NVXVO+2XNDRXUOXDR)+RHOXL XX3XU0XNPROP 

—-MASSXYGXRO + RHO/2XLXXGENVQXQO + RHO/2XLXXSXCNVXUOT... 
NVAIXLO ) 


MASS*XGX¥PO + MASSXYGXQ0 + RHO/2*LXX4XCNWPXPO+NWRXROD+... 


RHO/2*¥L¥¥3¥NVINNXVO 

=e Qu + TXxXQ0 9+ ZXIXYXPO +1YZ*¥RO + MASSXXGXWO+... 
RHO/2*L¥X¥5¥NPQXQO + RHO/Z2XL¥XGXCNPXUOTNWNPXWO ) 
-IYXPO + IX¥PO - 2*IXY*¥QO - IXZ¥RO + MASS¥YGXWOF+... 
RHO/2¥*¥L X¥X¥SXCNPQXPO+NQRXRO) + RHO/2XL X¥XGXNVQXVO 
IYZXPO -IXZ*XQ0 - MASS*¥XGXU0 -MASSXYGxXVO + ... 
RHO/2*LX¥¥5XNQR¥QO + RHO/Z2XL¥XGXCNRXUO +NWRXWO) 
(XGXHMEIGHT-XBXBOY)*COSCTHETA0)*COSCPHIO) 
—-(XGXHWEIGHT-XBXBOY)XSINCTHETAO)XSINCPHIO) +... 
(YGXWEIGHT-YBXBOY) xXCOS(THETA0O) 


COSCPSIO)XCOSCTHETAD) 

COSC ol OxXSINCTRenAGD <stNCPHLOo — SINCPSIO)*XCOSCPHIO) 
COSCPSIO)XSINCTHETAO0)XCOSCPHIO) + SINCPSIO)XSINCPHIO)D 
VOxCOSGr oun <STHCTREnAUXCOSCPAIO) + VOXSINCPSIO)X... 
SniGnnwo)  — HOxGUSCeESTOIxSINCTHETAQ)XSINCPHIO) + 
WOxSTNCrPS10) XCOSCPHIO) 


mUUACOSCrol0) <Sinet HerAdle + VO<COS(FSIO)XCOSCTHETA0)®... 


SINCPHIO) + WOXCOSCPSIO)*XCOSCTHETA0)*XCOSCPHIO) 


UGeciGr o) OOSCUSt THENAND — VOXSINCPSIG)XSINCTHETAO)DX... 


SNGE@lU) > VOxCO>oCEstOUXCOS(PFHIO) -— WOXSINCPSIO)*... 
SINCTHETAO)XSINCPHIO) + WOXCOSCPSIO)XSINCPHIO) 


SUNCESi0)xXCOSCTHETAD) 

SMvces lOve oINClHETAGXSINCPHIO) + COSCPS1I0)*COSCPHIO) 
Src oOo ST NCIMENAOD <euS(PHI0)=— COSCPSIOIJXSINCPHIO) 
VO ~SUNCP SLO) ASINCTHEDAUIACOS(CPHIO) - VOXCOS(PSIO)*... 
SINCPHIO) - WOXSINCPSIO)XSINCTHETAO)XSINCPHIO) - 
WOXCOSCPSIO)*COSCPHIO) 

=“UOXSINCPSIO)XSINCTHETAO) + VOXSINCPSIO)XCOSCTHETA0)®.. 
SLNCR HO) t+ WOxSTINCPSTUxXCOS(THETAO)XCOSCPHIO)D 
UOxXCOSCPSIO)XCOSCTHETA0) + VOXCOSCPSIO)XSINCTHETAO)X... 
CoCr se VOxXODN Cold xCOSCPHIC) + NOXCOS(PSIO)DX... 
SINCTHETAO)XCOSCPHIO) + WOXSINCPSIO)XSINCPHIO) 


-SINCTHETAO) 

COSCTHETA0)XSINCPHIO) 

COSCTHERAQ)XCOSCPHIO) 

VOXCOSCTHETA0) XCOSCPHIO)-WOXCOSCTHETAO)XSINCPHIO) 
-UDXCOSCTHETAOI-VOXSINCTHETAO)XSINCPHIO) -... 
WOXSINCTHETA0O)*XCOSCPHIO) 
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SINCPHIO)XTANCTHETAO) 

COSC PHIO)*TANCTHETA0) 

QOXCOSCPHIO)XTANCTHETAO) - ROXSINCPHIO)XTANCTHETAO) 
QOXSINCPHIO)/COS( THETA0)X]. 07 COSC THETAU) ee 
ROXCOS(PHI0)/7COS( THETA0) 1 .0/COSC THETADO) 


—/ NZ 


COS(PHIO) 
= Sa ice hase 
-QOXSINCPHIO) - ROXCOSCPHIO) 


SINCPHIO)7COSCIRNETAL? 

COSC PHI0)/COSCTHETAQ) 

QOXCOS(PHIO)/COS( THETA0)-ROXSIN(CPHIO)/7COS(THETADO) 
QOXSINCPHIO)/COSCTHETAO)XTANCTHETAO) + 
ROXCOS(PHIO)/COSC( THETA0) XTANCTHETAO) 
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WRITEC10,200)(CACI,J),J=1,12),1=51,12) 
CALCULATE THE B MATRIX 
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RHO/72¥L¥X3*¥XRDRXUOXRO+RHO/2XL XX2RC( XRDRXUOXVOFUOKR2X... 
2X XDRDRXDR) 

UOxXQOXXQDB/2 + UVUOXWOXXHDB/2 + U0XxX2*XXDBDBXDB 
UOXQO*XXQDB/2 + UOXWNOXXNDB/2 + UOXX2XXDBDBXDB 


los BooKos oe) 
DNGNPY 
Sw 
te 


UOXQO0*XQDS + UOXWOXXWDS +U0X*¥2*2XXDSDS¥DS+RHO/2RLRX3X... 


XQDSNXUOXQOXEPS + RHO/2XLXX2*XC(XWDSNXUOXWO + 2XXDSDSNX... 


UO0X*2*DS)XEPS 
RHOZ 2XLXX2xX0. 124CD0X00 1 2Z~ReM 
SINCTRHETAGD 


on™ 7 ™ 
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Ne Ne 


RHO/2XL ®X¥2*YDRXUOXX2 
-COSCTHETA0) £51 Titr BiG 


IN TN 
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UOxX*2*¥ZDB/2XRHO/2XLXX2 
U0XX2*XZDB/2XRHO/2XL ¥X2 
U0xX2*XZDS¥RHO/2XLXX2 + RHO/Z2XLXX2XZDSNXUOXX2XEPS 
=COS( THETAO) COS CEE GD 


-RHO/2*XLXX3XUOXX2XKDB/2 
RHO/2*L XX 3SXUOKX2RKDB/2 
-YBXCOSC(THETAO)XCOSCPHIO) + ZBXCOSCTHETAODXSIN( Pirie 


RHO/2*LXX3XUOXX2EMDB/2 
RHO/2*LXX3XU0XX2XMDBS2 

RHO 2*L¥¥3*C UOXX2XMDS+MDSNXUOXX2XEPS ) 
XBXCOSCTHETAO)XCOSCPHIO) + ZBXSINCTHETAQ) 
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woul 
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RHO/2XLXXS3¥NDRXUOXX2 
~XBXCOSCTHETAOJXSINCPHIO) = YEXSTNC Tei eoD 


FORMULATE THE A AND B MATRIX FOR STATE SPACE REPRESENTATION 
MULTIPLY MMINV AND DF/DX 


DO 80 I = 1,6 
DO 70 Jie= alc 
SUM = 0.0 
DO 60 K = 1,6 
SUM = SUM + MMINVCI,K)XACK, J) 
60 CONTINUE 
AACI,J) = SUM 
70 CONTINUE 
&0 CONTINUE 
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ran 
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50 
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MULTIPLY MMINV AND DF/DZ 


DOeo0) 1 = 1156 
DO 40 J = 7,12 
SUM = 0.0 
DO 30 K = 1,6 
SUM = SUM + MMINVCI,K)XACK, J) 
CONTINUE 
AACI,J) = SUM 
CONTINUE 
CONTINUE 


DOS =. 7,12 
DO6J=l, 
AACI,J) =A 
CONTINUE 

CONTINUE 


12 
(I,J) 


WRITEC1O,200)CCAACT,J),J=1,12),1=1,12) 
FORMATC 6E12.49) 


Ue ey’ SAND DF DU 


DOSII0 L1.- 1,6 
DO 100 J = 1,6 
SUM. = 0-¢ 
DO 90 K = 1,6 
SUM = SUM + MMINVCTI,KIXBCK, J) 
90 CONTINUE 
BoC) = SUM 
100 CONTINUE 
110 CONTINUE 
® 
x 
WRITEC 9,300) 0CBBCI,J),J=1,6),1=1,6) 
300 FORMAT C6E12.49) 
Xx 
x 
DO 405 I = 1,6 
READ (2,401) (GKK(I,J), J=1,21) 
405 HRoteso ,40LJCGKKCI,J), J=1,21) 
401 FORMAT C3E20.10) 
Xx 
Xx 
EESE 
END IF 
x 
x Peeemen orl ©2095 256,-1,1,1) 
x PRINTX,INFO,ISTRAT,IOPT,IONED, IPRINT,IGRAD,NDV,NCON,DX,... 
x OBJ,GW,IDG,NGT,IC,DF,AW,NRA,NCOLA,WK,NRWK,... 
x IWK, NRIEIK 
CALL DADSCINFO,ISTRAT,IOPT,IONED, IPRINT, IGRAD,NDV,NCON,DX,... 
VLB,VUB,OBJ,GW,IDG,NGT,IC,DF,AW,NRA,NCOLA,WK,NRWK,... 
IWK,NRIWK) 
Hee eNeOl ee ../0)) DELPRI=0.2 
* 
DERLVATIVE 
NOSORT 
Xx 
x 
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DOCU 
> UM 0.0 
Pom omk 
SUM 
CONTINUE 
XDOTUCJ) 
CONTINUE 
CALCULATE AAXX 
DOS ein = 
SUM O20 
DO V2ouK 
SUM 
CONTINUE 
XDOTXCJ) 
CONT TRUE 
CALCULATE XDOT 
DOWsl 
XDOT CJ) 
CONTINUE 
DO 35 J 
XDOTC J) 
CONTINUE 
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CALCULATE BBXU 
1,6 
1, 
SUM 


PART OF XDOT AAXX + BBXU 
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SUM 
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SUM + AACJ, KI ¥XCK) 


Sun 
AAXX + BBXU 
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XDOTXC J) + ALOUD 


he 
XDOTXCJ) 


UDOTM 
VDOTM 
HDOTM 


XDOT ¢ 
XDOT ¢ 
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PDOTM 


QDOTM 
RDOTM 
XDOTM 
YDOTM 
ZDOTM 
PHMDOT 
THETMD= 
PSMDOT= XDOT 
WRITEC3,600) 
INTEGRATE XDOT 


UM =INTGRL(6 
VM= INTGRLCO 
MM= INTGRLCO 
PM= INTGRL(CO 
Qi4i= INTGRLCO 
RM= INTGRLCO 
ArROsM INTG 
TROSM INTG 
ZPOSM INTG 
PHIM INTGR 
THETAM INT 
PSIM = INTGR 
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TLOThD 
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XC1) = UM 

X(2) = VM 

X(3) = IM 

XCG) = PM 

X(5) = QM 

X(6) = RM 

X(7) = XPOSM 

X(8) = YPOSM 

X(9) = ZPOSM 

X(10) = PHIM 

XC11) = THETAM 

X€12) = PSIM 

SUEPIH = ZORD = X(9) 

THMANG = X(11)%57.3 

UMOD(1)=DRM 

UMOD(2)=DBM 

DBPM= UMOD(3) 

UMOD(3)=DBM 

UMOD(G)=DSM 

UMOD(5)=DRPM 

UMOD(6 )=DBOY 

PHANG=PHIM/0.0174532925 

THMANG=THETAM/0.0174532925 

PSMANG= PSIMZ 0.0174532925 

PITCHM=THMANG 

DEPTH=-ZPOSM 

DBS = UMOD(2) 

DBP = UMOD(3) 

DS = UMOD(4) 

DR = UMOD(1) 

PUT IN STERN AND BOW PLANE STOPS 

HEGAESCDBS)).GT. 0.6) THEN 
DBS = 0.6XABS(DBS)/DBS 

ENDIF 

IFCABS(DBP).GT.0.6) THEN 
DBP = 0.6*ABS(DBP)/DBP 
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ENDIF 
IFCABS(DS).GT.0.6) THE! 
D>s= 0.6*ABS(DS)/DS5 

Ei Ouae 


INTGRD = CUMUMEVMXVM+HMXHIM+ PMP M4 QMXQMARMARTS « « 
He UAXPOSM# CYPOSM-YORD)XCYPOSM_YORE | 
TPO ORDDEP)X( ZPOSM~ORDDEP +PHINEE ET 
THETAMXTHETAM+PSIM%PSIM) a CDSMXDSM+ DBMXDBM+ DRMXDRM) 
OBJE = INTGRL(O. (0 .5)*¥INTGRD? 
OBJ = OBJ 


DYNAMIC 


ADDI 
VEnE 


OOK OK OOK 


KOK OK OK OK 


x AUV 
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S eTIME7 CEEN TIN? 10 o-DEE Woes 
RET IME/CEINTIM/20.-DEL 17 MODUOs? 
O=INTCRNDt1 

P=INy Cea s 

TPCO2Ge 1)? 0=10 

LpCe4ee. cl? p=20 


TIONALLY THE PLANES SHOULD BE AT EQUILIBRIUM SO THe 
elgg (hone PROCEED AT THIS NEW DEPTH WITHIN SOME TOLERANCE 


DSM=DX(0) 
DBM=Dx(10+0) 

TeCO.Ge. 10? DSM=0.000 
Teco. Ge.2 0 DBM=0.000 
DRM=DX(20+P) 
RPM=DX(30+0) 


CONSTRAINTS FOR A DIVE 


ORDERED DEPTH = ORDDEP 


GHC1) = (ZPOSM-ORDDEP)*®.2 

GUC2) = (ORDDEP-ZPOSM)*.- 2 

'S FINAL STATE MUST BE LEVEL FLIGHT AS FOLLOWS 
Ghic3) = THETAM 

GNC 4) -THETAM 

GWC5) PHIM 

GWC6) -PHIM 

GWC7) PS 

GWC8) ale SM 

GWC9) YPOSM-YORD)7/.4 

GWC10 (YORD-YPOSM)7 .4 

GHCLi = -ZPOSM 


now i i 


we New If 


AVOIDING THE OBSTACLE 


DIST1=SQRT( (XPOSM=XOBS 1) x(XPOSM: XOBS2 9 -- 
> POSM-ZOBS1)X(ZPOSM~ZOBS1)? 

he (DIST1.LT.DSAVE) DSAVE1=DIST1 

GWC6) = (1.-DSAVE1) 

HDX=XPOSM/17 .4925 

HDZ=ZPOSM/17 .425 

NDT=TIMEX6 ./17 .425 


TERMINAL 


x 
X66 


9000 


END 
STOr 


HRITEC11,66) XPOSM, YPOSM, DEPTH 
FORMAT (1% FlGe oan ree? 
IFCINFO.EQ.0) THEN 

PRINTX,O,P 

CONTINUE 

EESE 

ELE 

TECINFO. £9. 0D CALL ENDJOB 

CALL RERUN 
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FILE: The DSL Al 


TITLE RUN:16-5 NONLINEAR AUV MODEL 7 STERN PLANE AND BOW PLANE SEPARATED 
* (1) UPDATED:05720788 

* (2) RIGHT OBJ EQUATION 

* (3) ADS CONSTRAINTS ON DEPTH,PITCH,YAW,ROLL AND Y POSITION 


* (4) CORRECTED OBSTACLE AVOIDANCE ROUTINE 

KKK KKK KK KKKKKKKKKXKXXKKXADSL SET-UP % XX KKK KKK KKK KKK KKK KKK KKK KKK KKKKEKX 
FIXED ISTRAT, IOPT, IONED, IPRINT, INFO, IGRAD, NDV, NCON 

FIXED IDG, NGT, IC, NRA, NCOLA, NRWK, IWK, NRIWK, O, H,D,C,PP 

D DIMENSION AWC42,492) 

ARRAY WKC5000), IWKC1000) 

ARRAY DX(40), VLB(C40), VUBCG0), GWC11), DFC41), IDGC11), ICC11) 
PARAM NRA=42, NCOLA=42, NRWK=5000, NRIWK=1000 

PARAM IGRAD=0, INFO=0, NDV=40, NCON=11, NGT=11 

TABLE DXC1-2)=2%.0,DX03-40)=58%0., IDGC1-10)=10%-] 

TABLE IDGC11)=1%1 


Meee VEBCI=09)=-09X—-.17952, VLBC1lI-19)=09X—.2943,VLBC20)=0.,VLBC105=0. 
TABLE VUB(C1-09)=09%.174952, VUBC11-19)=09%.2493,VUB(20)=0.,VUBC10)=0. 
feeble Vibe 59)=19X-.62567,VUBC21-39)=19% 623627, VUBC460-41)=2x0. 


TABLE VLBC40-41)=2x0. 

PARAM ISTRAT=3, IOPT=1, IONED=1, IPRINT=0000 
INCON H=0, OBS1=0.,YZONE=0. 

HetnOD RECT 

CONTROL FINTIM=21.0,DELT=.10 

min | XPOS, 7P0S,2ZP05,PI1TCH, THEANG, DR, DS 

XRINT THETAD, W, DEPTH,PITCH, XPOS, DEPTH,NDX,NDZ,NDT 
ann) DS,DB;DR,DEPTH,PIYCH,XPOS, YPOS,ZPOS,NDT 
XAVE THETA,W,2,DEPTH,PITCH,DS,DB, BOWANG, STNANG 
XRAPHCDE=TEK618) TIME,DS 

Arar nC DE=TEK618) TIME, DEPTH 

XRAPHCDE=TEK618) TIME,WDOT 

XRAPHCDE=TEK618) TIME,W 

XRAPHCDE=TEK618) TIME, THETDD 

XRAPHCDE=TEK616) TIME, THETAD 

XRAPHCDE=TEK618) TIME,THETA 

mrearncDE-TEK618) TIME,PITCH 

XRAPHCDE=TEK618) TIME, BOWANG 

XRAPHCDE=TEK618) TIME,STNANG 

HHH K RR KRKRK RR KRRKXKRKKXKRXKXXNXXXXXDSL MODEL SET UP XX KKK KKK RK KKK KKK KKK KAKRKKK 


DIMENSION MMC6,6),6904),GK4(4),BR(04),HHC4) 
DIMENSION BC6,6),BB(C6,63 

DIMENSION AC12,12), AAC12,12) 

COMMOlie” ~BEOE Ma? SEClZ)> FRC6), MMINV(C6,6), UCFC4) 
muAED DWG TA; IDGT, TIERS LAST, I,K,M, JJ, KK, 1 

INTEGER 

ARRAY WKAREAC5G), XC12) 

Xx 


Wo0W * 


X 

CONST 

x 

x LONGITUDINAL HYDRODYNAMIC COEFFICIENTS 

X 

CONST XPP = »XQQ = »XRR = »XPR = pe 
XUDOT= »XWQ = »XVP = »XVR = pe 
XQDS= »XQDB= »XRDR= »XVV = re 
XW = »XVDR= »XWDS= » XWDB= ye 
XDSDS= » XDBDB= »XDRDR= »XQDSN= ps 
XWDSN= »XDSDSH= 

x 


67 


x 

x 

XONST 
XONST 
mS 
x 
x 


LATERAL HYDRODYNAMIC COEFFICIENTS 


YPDOT= ,YRDOT= = 
YVDOT= ,YP. = ae 
YHP = SYHR = ,YV = 
YDR = ,CDY = 

NORMAL HYDRODYNAMIC COEFFICIENTS 
ZQDOT= ,ZPP = : 
ZVIDOT = ,ZQ = cae = 
740 = Do = 72S = 
ZQN = ,ZWN = ,ZDSN= 


ROLL  HYDRODTNAMIC COERPPIU TENT. 


KPDOT= » KRDOT= »KPQ = 
KVDOT= » KP = »KR = 
KHP = » KWR = »KV = 
KPN = » KDB = 

PITCH HYDRODYHWAMNIC COEREDCI ENT: 

MQDOT= » MPP = »MPR = 
MDOT = » MQ = »MVP = 
MIN = » MVV = »MDS = 
MQN = » MN = »MDSN = 
YAW HYDRODYNAMIC COEFFICIENTS 

NPDOT= » NRDOT= »NPQ = 
NVDOT= » NP = »NR = 
NWP = » NWR = sNV = 
NDR = 


»YQR 
»YVQ 
»YVW 


»ZRR 
»Z2VR 
» 2DB 
»CDZ 


»KQR 
»KVQ= 
»KVEA = 


»MRR 
»MVR 
» MDB 


»NQR 
»NVQ 
»NVW 


MASS CHARACTERISTICS OF THE FLOODED MARK@IX VEnTCrE 


WEIGHT = » BOY = »VOL = 
YG = , £26 = »XB = 

IX = » IY = »,1Z = 

LS aN » IXY = »YB = 
Ee » RHO = »G = 

AQ = »KPROP = »NPROP = 
DEGRUD= 0.0 FUEGSTN=  70m0 

XOBS1=36.0 

Z05S5i=41240 


INPUT INITIAL CONDITIONS HERE Ir {REQUIRED 


? 


»XG 
2B 
,IXZ = 


»NU = 
XITEST= 


ve we Ve Ww NW NW 
° ° ° ° ° ° 


INITIA 
5 
NOSORT 


x 


KK MK KK MK 


«KK MK 


L 


DSAVE1=SQRT( CXPOS-XOBS1 )*(XPOS-XOBS1)+(ZPOS-Z0BS1)xCZPOS-Z0B51)) 


ORDDEP 


Sey 425> 


YORD=40.0 


D=0 
intaie | 
Pech. c 


U 
V 
W 
p 
Q 
R 


(on Tew Ten em i ew) 


LATYAW 
NORPIT 


Q.1) THEN 


Oa, 


(on en i an i on i en) 


in - 


e0 
0.0 


RE = UOXL/NU 


SDUy = 


Ose 5et Cle 2IGE=17 IXCRE = 22E7 )XX2 


DERE CeNnGih FRACTIONS FOR GAUSS QUADUTURE TERMS 


G41) 
G4(2) 
G4(3) 
G4(4G) 


DEFINE 


GKGC1) 
GKGC2) 
GK4(3) 
GK4(4) 


DEINE 


BR(1) 
Bie? 
BRC 3) 
BRCG) 


0.069431844 
O-s50009G76 
Oeeo77oUa2! 
a9 50566155 


WEIGHT FRACTIONS FOR GAUSS QUADUTURE TERMS 


On 73927 42c0067 
pocoUTcor? 4512 
Umoace07 cor? 4ol2 
O17 S927 4225657 


THE BREADTH BB AND HEIGHT HH TERMS FOR THE INTEGRATION 


Pont 7 v2 
Tee rae 
ys en a 
JoeUS7 IZ 


Wout oat oat 
= 


in © 


KK KK OK KR 


HAC)) =s16 367i 
nnC2) = oo le 
HC Ss) = seo le 
HHCG) = 23.76/12 


MASS = WEIGHT/G 


DIVAMP = DEGSTNX0.0174532925 
RUDAMP = DEGRUDx*0.0174532925 


N = 6 
DO 15 J = 1,N 
DO 10 K = 1,N 


MMINVOJ,K) = 0.0 

AA ey de = ee0)) 0 

CONTINUE 
CONTINUE 


MMC1,1) = MASS =CCRHO/ CI ACE XXSO xe 
MMC1,5) = MASS*ZG 

Mil 6) = =haAsoenG 

MMC2:2) = MASS. =CCRHOZZ XC LXXSU SO 
MMNC2,4) = -MASSXZG -CCRHO/2) CL XG )XYPDOT) 
MMC2,6) = MASS®XG - (CRHO/2)*CL¥X4)XYRDOT) 
MMC 335) = MASS = (CRHOZ CXC LXXS02ZuOCTD 
MMC3,4) = MASS*YG 

MMC3,5) = -HMASS*XG -CCRHO72 YXCEXxGoxZQpeT) 
MMCG,2) = -MASSXZG - CCRHO/2)*(LXXG)XKVDOT ) 
MMCG,3) = MASSX®YG 

MMCG,4) = IX - CCRHO72) CL 5) XKPDOT) 
MMCG,5) = -IXY 

MMC9,6) = -I[XZ -CCRHO/ 2) XC LXXSIAKROGTD 
MMC5,;1) = MASSxZG 

MMC5,3) = -MASS*¥XG -CCRHO/2)*CLXX4) XMWDOT )D 
Mi@ 5:74)" = = IY 

MMC5,5) = ITY -CCRHOZ2)X( LAXS)DxXMOQRGTS 

MMC S76 oP =. = ieZ 

MCG 712) =) =Hisoxy 6 

MM(C6,2) = MASS¥®XG -CCRHO/2)X(L*XG )XNVDOT) 
MMC6549) = -IXZ — CCRHO/2)X(LXxa xeon 
MMC 6m 20). =) = lie 

MMN(6,6) = 1Z - (CRHO/2)X(LXx5)xXNRDoT) 


8 


LAST = NX¥N+33N 

DORZ0 f= 1,LAST 

WKAREACM) = 0.0 
20 CONTINUE 


x 

IER = 0 

IA = 6 

IDGT = 4 

HRITEC 87500) CCM JG) v= clo 6) 
x 

CALL LINV2F(MM,N,IA,MMINV, IDGT,WKAREA, IER) 
x 


WRITEC 8,GO00)CCMMINVCI,J), J = 1,6),I1I = 1,6) 
400 FORMATC6E12.49) 
x 


EESE 
ENDIF 


aK OK 


CAEL DADSCINEO, ISTRAI,.IOPT,IONEDZIPRINT, IGRAD,NDV,NCON,DX,.. 
VLB,VUB,OBJ,GHW,IDG,NGT, Le, DF,AW,NRA, NCOLA,WK, NRWK, wes 
IWK, NRIWK) 
iF CH eee eet nen 
WKCI2) = .002 
CAme DADS CL NFO stRAI,10P1;,10NEDSIPRINI,IGRAD,NDV,NCON,DX,... 
VLB,VUB,OBJ,GW,IDG,NGT,IC,DF,AM,NRA,NCOLA,WK,NRWK,... 
IWK, NRIWK) 
Polio; EQsU DELPRI Or 
POliiGe. £Q20o. DELPLT OFZ 


I 
if 


KK KK ODOINY 


DERIVATIVE 
NOSORT 


PROPULSION MODEL 


aK OK OOK OOK OOK 


SIGNU = 1.0 

Meee rt. 0.0) SIGNU = -1.0 

Meee socU) acts. X1TEST) U = XLIEST 

SIGNN = 1.0 

iewCREMoet.0.0) SIGNN = -1.0 

ETA = 0.012*®RPM/U 

RE = UXL/NU 

CDO pUdseoe+ CIS Z9I6E—-l7 7 2ORE.— 1.2ZE7 )¥*2Z 
Ci 0.008*L¥X2XETAXABSCETA)/(CADN) 

eae 0.008*L¥*2/(AO0) 

ERs -]1 .O+SIGNN/SIGNUXCSQRTCCT+1.0)3-1.0)/7C(CSQRTCCT1+1.0)-1.0) 
XPROP = CDOXCETAXABSCETA) - 1.0) 


CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER THE VEHICLE 
INTEGRATE USING A G TERM GAUSS QUADUTURE 


KOK OK OK OK OK OK 


LATYAW = 


CV+GGCKIXRELIXX2 + CW-GOCK) QRL )X¥2) 


»4 
UCFCK) = SQRT 
Cievbes 0) ten 


— oN 


74 


PIU. 


© 
© 


mK OK OK 


KOK OK 


«KK OK 


TNLO DSe Al 
TERMO = CRHO/2)*C(CDYXHHCK)XCV+GGCK)XR¥L)XX2 +... 
CDZ¥BRCK) XCW-G4(K) XQXL ) ¥¥2) 
TERM] = TERMOX(CV+G4(K) ¥R¥L)/ZUCFCK) 
TERM2 = TERMOXCW-GGC(K) ¥Q¥L)/UCFCK) 
LATYAW = LATYAW + TERM] XGKGCK) XL 
NORPIT = NORPIT + TERM2*GKGCK)XL 
END LT 
CONTINUE 


FORCE EQUATIONS 


LONGITUDINAL FORCE 


ela) OO 


MASS¥VXR - MASSXWXQ + MASSXXGXQkX2 + MASSXXGEREX2-... 
MASS®YGXPXQ = MASS¥ZGXPXR + CRHO/2)XL ¥X4%( XPPRPXXZ ee 


XQQXQKK2 + XRRXRXX2 + XPRXPXR) +CRHO/2Z) XL XX3RCXWQHNKQ +... 


XVPXVXP+XVREVERtURQK( XQDSXDS+XQDBXDBI+XRDRXUXRXDR)+... 
CRHO/ 2) KLXX2XCXVVKVRRS + XWINKNHX2 + XVDRXUXVEDR + UXWX... 
CXWDS*XDS+XWDBXDB)+UX*2*( XDSDS¥DS¥¥2+XDBDBXDBXX2+... 

XDRDRXDR¥X¥2))-CWUEIGHT -BOYIXSINC THETA) +¢€RHO/2)xXE Axes 


XQDSNXUXQXDSXEPS+( RHO/2) XL¥X 2% ( XHDSNXUXWXDS+XDSDSNXUXX2X. .. 


DS¥*X2)¥EPS +CRHO72) XL ¥X2XUXX2*XPROP 


LATERAL FORCE 


Ee Ge) 


NORMAL 
FRCS) 


F 


—~MASSXUXR - MASSXXGXPXQ + MASSXYGXRXX2 - MASS¥ZGXQER +.. 
CRHOZ2 XL EXGXCYPQHPXQ + YQR¥Q¥RI+CRHO/2Z)XLXX3¥CYPRXURP +.. 


YRXUXR + YVQXV¥Q + YWPXWXP + YWRXWXR) + CRHO/2)*L*X2*% 


CYVXUXV + YVWXVXH +YDRXUXxX2XDR) -LATYAW +CWEIGHT-BOY)X.... 
COSC THETA) XSINCPHI) 


ORCE 


MASSXUXQ - MASSX*VXP - MASSXXGX¥PXR - MASSXYGXQXR + 
MASSXZGRPXX2 + MASSXZGRQRX2 + CRHO/2) XL ¥XGRCZPPRPRR2 +, 
ZPR¥PXR + ZRR¥RXX2Z2) + CRHO/2Z) XL¥X3XCZQRUXQ + ZVPXV XP ee 
ZVREVKXR) +CRHO72)XLXR2ZKCZWHUXWN + ZVVEVRR2 + UXX2*(ZDSX... 
DS+ZDBXDB) )-NORPIT+CWEIGHT-—BOY )XCOS( THETA) XCOS( Plo aaee 
CRHO/2) XL ¥XX3XZQNRUXQHXEPS +CRHO/2) XL XX2XCZWNXURW +ZDSNX... 
UX*X2*XDS)XEPS 


ROLL FORGE 


FP(4) 


~IZXQRR +1YXQRR -IXYXPHR +1IYZRQHX2 -ITYZRRRR2 +1XZRPRQ 4.70, 
MASS¥YGRUXQ -MASS¥YGHVXP -MASSXZGRWXP+(CRHO/2)XLXXSXCKPQH... 
P¥Q + KQRXQ¥R) +CRHO/2)¥L¥XXGX(KPXUXP +KRXUXR + KVQHVEQ +... 


KMIPXWxP + KWR¥WXR) +CRHO/2)*XLXX3XCKVXUXV + KVWXVXW) +... 
CYGXWEIGHT - YBX¥BOY)*COSCTHETA)*COSCPHI) - CZGXWEIGHT —- 
ZBX¥BOY)XCOS(CTHETA)XSINCPHI) + CRHO/2)XLXX4XKPNXUXPXEPS+ 
CRHO/Z2) *LXX3SXUXX2XKPROP +MASSXZGRUXR 


2 


*K OK OK 


AK OOK OK OK OK OK Ok 


* OK KOK OK OV 


KOK OOK 


* KK 


PITCH FORCE 


FPC5) = -IXxpxpR +IZ¥P¥R +IXYXQKRR =IYZ*PXQ ~IXZ¥PxXx2 +IXZ¥R¥X2 -| 


MASS*XGXUXQ + MASSXXGX¥VxP + MASSXZGXVXR - MASSXZGXHXQ +. 


XBXBOY )¥COS( THETA) XCOS( PHT) oar ae 
CRHO/2) XL XG XMQNXUXQXEPS aes 
Bee) XL xx 3% (MINX UXH+MDSNXUXX2*DS )XEPS- . 
(CGXWEIGHT-ZBxBOY) SINC THETA) 


YAW FORCE 


FP(6) = -lyYxpxg PMS ee 0) +IXYXPXX2 ~IXYXQ*x2 +IyZxpxp ~IXZ*¥QXR -.,, 


MASSXXGXUXR + MASSXXGXWxP - MASSXYGXVXR + MASSXYGRHXQ +... 
CRHO/2)XL*X5%*(HPQXPxQ + NQRXQXR) +CRHO/2 XL ¥xGx(NPxXUXP4 | 
NRXUXR + NVQXVXQ +NWPXHXP + IWR XW XR ) + CRHO/2 XL XX 3x (NV. 
UXV + NVNXVXW + NDRXUXxX2xDR) - LATYAW + CXGXWHEIGHT - .. 


+CRHO/2 IS Ss Se GTADaSINCPHI D+ CYGHHEIGHT xSINCTHETAy 
TANHO?2 J&L XX 3XUXK2XHPROP-YBXBOY RSS He rere 


TF(Z.E@.50.0)THEN 
WRITE C5, 500)(FPCI), I= 1,6) 
= 0 


Z = 

END IF 

NOW COMPUTE THE F(1-6) FUNCTIONS 
DORSOO J = ) ¢ 


FCJ) = 0.0 
DO 600 K = 1,6 

Cy te MMINVOJ,K)*FPCK) ee) 
CONTINUE 


ERESUAST ISI xX EQUATIONS COME FROM THE KINEMATIC RELATIONS 
FIRST SET THE DRIFT CURRENT VALUES 


UCO = 0.9 
VCO = 0.0 
WCO = 0.9 


INERTIAL POSITION RATES FC7-9) 

em UCG + UXCOS(PSI)*COS( THETA) a VXCCOSCPSI)XSINCTHETA)x. __ 
SINCPHI) - SINCPSI)*COS(PHT) ) a NXCCOSCPSIXSINCTHETA)x.__ 
COS(PHI) + SINCPSI)XSINCPHT)) 

PS) ere) UXSINCPSI)XCOS( THETA) a VXCSINCPSI)XSINCTHETA) x. __ 
SINCPHI) + COSCPSI)*COS(PHT) ) + WXCSINCPSIXSINCTHETADX. |. 
COSC(PHI) - COSCPSI)XSINCPHT)) 


FO?) = Heo. UXSINCTHETA) *VXCOS (THETA )XSIN( PHT) +WXCOS(THETA)x. . 
COSC(PHI) 


EULER ANGLE RATES ClO) 

FC1l0) = p 4 @XSINCPHI )¥TANCTHETA) + RXCOS (PHI )XTAN( THETA) 
et) = QXCOS(PHI) - R¥SINCPHI) 

ml. = @XSINCPHI )/COS( THETA) ar RXCOS( PHI )/COS( THETA) 


KKK OK OK OOK OK 
[a] 
So 


IF (Z2.EQ.1.0)WRITE (9,500)(F( 1) ie ieee 
FORMATC(6E12.4) 


CR aa 

UDOT = F(1) 

VDOT = FC2> 

WDOT = FC3) 

PDOT = FCG) 

QDOT = FC5) 

RDOT = F(6) 

XDOT = F(7) 

YDOT = F(8) 

ZD0T=3F Cs) 

PHIDOT = F(10) 

THETAD = FC1]1) 

PolvUCT = ECcl2Z) 

Ue= INCRE CUS DoT) 
XC1) = VU 

V = INTGRLCO.0,VDOT) 
X€2) = V 

Mo = INTGRECO. 0,067) 
X(C3) = W 

P = INTGRL(0O.0,PDOT) 
ACG) = Ae 

Q = INTGRLCO.0,QDO0T ) 
XC5) = Q 

R = INTGRLCO.0,RDOT) 
X(6} = R 

XPOS = INTGRL(0.0,XDOT) 
X(7) = XPOS 

YFOS. = TNTGRiUC0, 03 yDer) 
X€8) = YPOS 

Z = INTGRECG. 0,200) 
X09) = ZPUS 

PHI = INTGRLCO.0,PHIDOT) 
AC1O) = Eid 

THETA = INTGRL(O.0,THETAD) 
XC11) = THETA 

PSI = INTGRLCO.0,PSIDOT) 
XCl2) = Psi 

PHIANG = PHI70.01749532925 
THEARG = THETAZ 0 01796532925 
PSITANG =" FSI/ 0.017 4552925 
ZP Os se2 

DEPTH=ZPOS 


PITCH=THEANG 

BOWANG=(C DBZ .01745) 

STNANG=CDS7 .017495) 

INTGRD = CUXU+VXV+WXWN+PXP4+QXQ+RXR+XPOSXXPOS+CYPOS-YORD)IX. 
CYPOS-YORD)+(Z-ORDDEP) XC Z-ORDDEPO+ PAT Pal ee 
THETAXTHETA+PSIXPSI) + CDSXDS+DBXDB)+(0DRXDR) 

OBJ1 = INTGRUC O83 0. 5)xXTHTGE De 

OBJ = OBJI 





x 
DYNAMIC 


mK OK OK OK 


K KK OK OK 


RNS iinewer HTIMN’ LOS=-DEL T7700 
Peete? (PINT IM720.-DELT/100 
O=INTCRND+] 

PP=INTCPN)+41 

PCO; GE.10) O=10 

Page JOb.2c0) PP=c0 


00.) 
no) 


ADDITIONALLY THE PLANES SHOULD BE AT EQUILIBRIUM SO THE 
VEnlClUbELtL PROCEED AT THIS NEW DEPTH HITHIN SOME TOLERANCE 


DS=DX(0) 
DB=DX(10+0) 
PaG07CeE. 10) D5=0. 
Teco. GE. LO) DB-O'. 
DR=DXC20+PP) 
RPM=DXC30+0) 


CONSTRAINTS FOR A DIVE 


ORDERED DEPTH = ]0KDDEP 


GWC1) = (CZ-ORDDEP)DX.5 
GNCzZ) = CORDDEP-Z)*.5 


meV 'S FINAL STATE MUST BE LEVEL FLIGHT AS FOLLOWS 


KK KK OK OK 


GhiC3) = THETA 

GHWCG4) = -THETA 

GHC5)= CYPOS-YORD)7.4 
Giieow= CYORD-YP0S37.4 
GHC7)=PSI 

ChieS) —=Pol 

GWC9)=PHI 

GWC10)=-PHI 
GNC11)=ZP0S 


AVOIDING THE OBSTACLE 


DIST1=SQRTC CXPOS-XOBS1 )*XCXPOS-XOBS1)+(ZPOS-ZOBS1 )x*( ZPOS-ZOBS1)) 
Maelo. ti. DSAVEI> DSAVEI=DIST1 
GNC6) = (€1.-DSAVE1) 


NDX=XP0S/17.4925 
nia —-2h 05717 .425 
NDT=TIMEX6 .4717.425 


TERMINAL 


x 
m9 99 
9000 


END 
STOP 


PECINFO.E@.0) THEN 
PRINTX, DSAVE1 
FORMATC1X,E15.9) 

CONTINUE 

BES E 

pf DL 2 

IFCINFO.EQ.0) CALL ENDJOB 
CALL RERUN 


ks 


Fittees A Dot Al 

TITLE LINEAR AUV MODEL 7 STERN PLANE AND BOW PEANE SEPARATED 

TITLE WITH COMMANDS TO NONLINEAR MODEL 

* (1) UPDATED: 05730786 

* (3) RIGHT OBJ EQUATION 

* (4) ADS CONSTRAINTS ON DEPTH AND PITCH 

* (5) OBSTACLE FURTHER DOWN THE TRAJECTORY AND ABOVE IT 

* (6) CORRECT OBSTACLE AVOIDANCE ROUTINE ADDED 

KKK KKK KKK KKKKKKKXXXKXKADSL SET-UP XX KK KKK KK KK KKK KKK KK KK KK KKK KKK KKK 
FIXED ISTRAT, IOPT, IONED, IPRINT, INFO, IGRAD SSDV eos 

FIXED IDG, NGT, IC, NRA, NCOLA, NRWK, IWK, NRIWNK, OF Hp DP Gyre 

D DIMENSION AWC42,492) 

ARRAY WKCG000), IWKC1000) 

ARRAY DxX(G0), VLBC40), VUBC40), GNC07), DFC41), IDGC07), ICC07) 
PARAM NRA=42, NCOLA=42, NRWK=4000, NRIWK=1000 

PARAM IGRAD=0, INFO=0, NDV=40, NCON=07, NGT=07 

TABLE DXC1=2)52%.0,DXC3-21)=19X%0., TDGGieee ec -e1 

TABLE DX€22-40)=19x0. 

TABLE IDGC7-0)=1%1 

TABLE VLBC1-9)=9X-.17452, VLBC11-19)=9X-.24943,VLBC10)=0 .VUb Cause 


PARAM 
INCON 


METHOD RECT 
CODTROL Pit Zin, 


PRINT 
*XRINT 


XRIWT. YPOSMewees 


AR 
XAVE 


XRAPHCDE=TEK618 ) 
XRAPHCDE=TEK618) 
XRAPHCDE=TEK618) 
XRAPH( DE=TEK618) 
XRAPHCDE=TEK618) 
XRAPH( DE=TEK618) 
XRAPH( DE=TEK618) 
XRAPHCDE=TEK618) 
XRAPHCDE=TEK618) 
XRAPH( DE=TEK618) 
KKK XX KKK RK KK KXKKXXXD S L MODEL FOR LINEAR SIMULATION XXXXX¥X¥XXRXRKKKK XX) 
HHH KI KK KKK KAND 


X LINEAR MODEL7NON-LINEAR MODEL 

D COMMON/BLOCK1/7 MMINV(6,6), MMC6,6), AAC12,12), BBC6,6) 
D COMMON’ BLOCK2/7 B(6,6),AC12,12), UMOD(6),GKK(6,21) 
D COMMONZ BLOCK3S7 FC12), FROG; Uereg 

D COMMON/BLOCKS7 G4(04),GK4(4),BRC4),HHC4 ) 

D COMMON/BLOCKS5Z XDOT(C12),XDOIX( 1297 poe» 

FIXED W,IA,IDGT,IER,LAST,J,K,M,JJ,KK,1 

INTEGER 

ARRAY WKAREAC5G), X€12) 

% 

* 

* 


VUBC1-9)=9%.174952, VUBC11-19)=9%.2443,VUB(10)=0.,VUBC20)=0. 

VLBC21-39)=19%-.523627,VUBC 21-39) =19%X% . 523627 , VUBC G0 =4)) ee 

VLB(40-41)=2x0. 
ISTRAT=3, IOPT=1, IONED=1, 
n- Up OBS =055 720-0. 


DELT=.1 
XPOS; TYROS; 2P 05, AP05M, YPOSM. ZR0Sn 
DS,DSM, DBPM, DBP, PITCHM, PITCH, XPOSM, YPOSM, XPOS, YPOS, ZPOSM> 2) Gane 


IPRINT=0000 


THETAD,W, DEPTH,PITCH, XPOS, DEPTH, NDX,NDZ,NDT 
THETA,WN,2Z,DEPTH,PITCH, DS,DB,BOWANG, STNANG 
TIME? Ds 

[euler Dele ios 

TIME, MDOT 

[Pllyia se 

Litt. (herp 

EIMES THETAD 

TIRES nels 

a Me.k Lem 

TIME, BOWANG 

TIME,STNANG 





NON-LINEAR SIMULATION VARYING CONTROL LAINXXXXXXXXxX? 


CONST 
X 


x LONGITUDINAL HYDRODYNAMIC COEFFICIENTS 

x 

CONST XPP = »XQQ = »XRR = »XPR = 
XUDOT= »XWHQ = »XVP = »XVR = 
XQDS= »XQDB= »XRDR= »XVV = 
XW = »XVDR= »XWDS= »XHDB= 
XDSDS= » XDBDB= » XDRDR= »XQDSN= 
XWDSN= »XDSDSN= 

X 

x LATERAL HYDRODYNAMIC COEFFICIENTS 

¥ 

CONST YPDOT= »YRDOT= »YPQ = »YQR = 
YVDOT= »YP = »YR = »,YVQ = 
YHP = »YWR = »,YV = »YVW = 
YDR = »CDY = 

x 

* NORMAL HYDRODYNAMIC COEFFICIENTS 

x 

CONST ZQDOT= Pore. = »2PR = »ZRR = 
ZIIDOT= ,2Q = »ZVP = »2VR = 
Z\ = Perate = eoUS = »2DB = 
ZQN = »,2WH = » 2DSN= pe DZ = 

x 

x RUceeaty DRODYNAMiC COEFFITIENTS 

x 

CONST KPDOT= » KRDOT= »KPQ = »KQR = 
KVDOT= » KP = »KR = »KVQ= 
KHP = » KWR = »KV = »KVW = 
KPN = » KDB = 

Xx 

x PITCH HYDRODYNAMIC COEFFICIENTS 

¥% 

CONST MQDOT= » MPP = »MPR = »MRR = 
MWDOT= » MQ = »MVP = »MVR = 
MW = » MVV = »MDS = »MDB = 
MQN = » MWN = »MDSN = 

x 

x YAW HYDRODYNAMIC COEFFICIENTS 

¥% 

CONST NPDOT= » NRDOT= »NPQ = »NQR = 
NVDOT= » NP = »NR = »NVQ = 
NWP = » NWR = »yNV = »NVW = 
NDR = 

X% 

¥ MAS SeCHARACTERIS1T1CS OF THE FLOODED MARK IX VEHICLE 

x 

CONST WEIGHT = » BOY = »VOL = »XG = 
YG = » 26 = »XB = ,2B = 
IX = ae: »,ilZ = »,1XZ = 
IYZ = » IXY = »,YB = 
Ps Pes = »,G = »NU = 
AQ = Phe ROr. = »NPROP = , MIT EST= 
DEGRUD= 0.0 »DEGSTN= 0.0 

Xx 

¥% 


CONST X0OBS1=36.0 
Behol ZOBSI= -12.0 


nae 


~“ ~ “ ~ 
°° e 


~“ ~~ “ “ “ 
e . e ° e ° 


x INPUT INITIAL CONDITIONS HERE TESREGU ERED 
x 
NITIAL 


IL 
~ 
x DSAVE1=SQRTCCXPOSM-XOBS1 )*CXPOSiiOe se 
x (ZPOSM-ZO0B51)x( ZPOSM-ZOESiee 
* DSAVEV=DSAVE1 
x INITIALIZE ALL MATRICES AND ARRAYS TO ZERO 
NOSORT 
ORDDEP=20.0 
YORD=40.0 
D=0 
H=H+] 
TF CHOEQ.1) THEN 
N = 6 
DO 2 J =1,N 
JJ= J+ 
DO 1K = 1,N 
KK= Kh 
KKK= KK + N 
MMINVCOJ 


GKKCJ,KK 
CONTINUE 
CONT LNVE 


KK KN 


DMPUT THE LINEARIZATION POLHNT SPAR sAViEweRs 


V0 


oo a .e'ea ea cS 
Oooo 
© 


“ 
cS 
= 
" 
Tn =) 
oo Oc. 


(a) 





KK OK OK OK OK OK 


*K XK 


mK OK OK 


2K OK OK 


Tipe eine MODEL STATES INITIAL CONDITIONS 


a2) 
= 
mom ut eae 


=e 
a0 
© 
W 
tho out tt 
- Ooo 
ooo 


INPUT THE VEHICLE INITIAL CONDITIONS 


Thi TALL Zee es CONTKOES 


DEEBOY--050 
DBOY=0. 
DS= 0.0 
DSM=0.0 
DBM=0.0 


LATYAW 
NORPIT 


A 
0 
= 
is 
ine 
rroo 
co) 
=) 


MASS = WEIGHT/G 


DavAriE = DEGSTN«ZU. 0174552925 
RUDAMP = DEGRUDX0.0174532925 


DEFINE LENGTH FRACTIONS FOR GAUSS QUADUTURE TERMS 


G41) = 0.069431844 
G4(C2) = 0.330009478 
G4K3) = 072669990521 
G44) = 0.930568155 


DEFINE WEIGHT FRACTIONS FOR GAUSS QUADUTURE TERMS 


GK401) = 0.1739274225687 
Grace) = 0252607 25/774512 
Oreo = 0532607 257 79512 
GK4(04) = 0.1739274225687 


es 


KK OK OK OK 


b 4 


DEFINE tne 


Bin Cl ere 
BRCZ)> 75 
BRCS) = 75 
BR(G) = 55 
HHC1) = 16 
HAHCZ = Sl 
Bhto d= sol 
HH(G) = 23 


THE LINEAR 


PTA i) (oth 
ETA Leo 

RE = UOXLZ 
CDO .00 
CT 0.00 
Cit O00 
EPS -1.0 
XPROP =: CD 


on Wow 


CALCULATE 


NJ NO AY a 

~ we 

Nes es Ny we 
uot 6 ee 


~. ew ew Y .~ ww 
ry | | oat al 


oot mt bat 


Nee Nee NY NY 


LAST = NXN 
DO 20 M = 
WIKAREACM) 
CONTINUE 


BREADTH BB AND HEIGHT HH TERMS FOR THE INTEGRATION 


SIT Le 
ee ee 
1 Ze2 
.08/12 


SSF 12 
Pe O7 lic 
-Oo7 iz 
SLO ue 


PROEULS TONG ODE 


2*XRPM/U0 


NU 

985 + (1.296E-l/ JACKE el 2b 
SXLEX2XETAXABSCETA)I/ (CAD) 

BXL¥XX2/ (AN) 

+CSGk 1 Celt] 20 0=i a Oe ee 0) =i0)) 
OXCETAXABSCETA) = "10) 


THE MASS MATRIX 


MASS -CCRHO/2) XC L¥X3)XXUDOT ) 
MASS*ZG 
-MASSXYG 


MASS =CCRHOZ2)xXCU4xs0 47 Dol 
-MASS®ZG -CCRHO/42)*(L¥XG)*YPDOT) 
MASS®XG - CCRHO/2) XC L*XXG)XYRDOT ) 


MASS - CCRHO/2)*(L¥*X3)XZbIDOT 
MASS*YG 
-MASS®XG -CCRHO/2) XC L*X4)X*ZQDOT) 


-MASS®ZG - CCRHO/2) (LG) *XKVDOT) 
MASSX*YG 

IX = CCRHO7 2) ¥CLXXSDXR EDO 

SLAY 

~IXZ -CCRHO72)*CL*¥*¥5)XKRDOTD 


MASS¥®ZG 

-MASS*®XG -CCRHO/Z2)*C(L¥*G)*MNDOT) 
-IXY 

ITY -CCRHO42)*C(L¥*¥5)XMQDOT ) 

-“lyYZ 


aMASSA1G 
MASS¥XG -CCRHO/2)*CL¥XG)*NVDOT) 
~IXZ = (CRRO/2)XCEXXS07 hie bee 
me 
TZ - (CRHO/2)X(L**5)*XNRDOT) 


+3%N 
to bAST 
= 026 


80 


KK KOK OK 


CALL tTNVZECMM, Hl, IA,MMINV, IDGT, HKAREA, IER) 
CALCULATE THE A MATRIX FOR THE LINEAR MODEL 


AC1,1) 


AC1,2) 
AS) 
AC1,4) 
AQ1,5) 


AC 1,6) 
AC1,11) 
AC2,1) 
AC2,2) 
AC2,3) 
AC2,4G) 
AC2,5) 
AC2,6) 


AC2,10) 
AC2,11) 


AC3,1) 


AC3,6) 


= 
= 
-_ 
= 
= 
— 


-_- 
—_ 


Wo out 


RHO? Px xan og DSXDS#Q0+XQDB/2xDBPXQ0+XRDRXROXDR)4.__ 
FEU OS Cen ae G XY DREV OXDR+XHDSXDSXWO+XHD Bo ore ook i cae 
2XU0X( XDSDSXDSxx2 + XDBDB/2*DBPxx2 + XDRDRXDRxx¥2) )+ oe 
2 XD SDSHE OO eon x GOXDSXEPS#RHO/2*L xx2% OXHBore oc bot 
KODE PDB Soa pane EPS RHOXLXX2xUOXXPROPTRHG oe ooty 
XDBDB Dene op RNO/ 2x1 xx2xXNDB/2xDBSHHOTRHR exe ote 
XDBDB/2XDBS*x2 

MASS*RO+RHO/2XL XX3%( XV PxPQ4 XVR¥RO) + RHO/2xL xx2x 
C2¥XVVXVO + XVDRXUOXDR) 

~MASS*QO + CD Soe nox CXMOXQ0)+RHO/2XLXX2%(2%XHHXNOF. 
A DSxDSxU0+ (XWDB/2xDBP+XWDB/2*DBS 5x4, +XWHDSNXUOXDSXEPS ) 
~MASS*YGXQ0-MASSxZG%R 04 RHO/2*LX%4%(2*XPPXPO+XPRXRO). 
w RHO/2X*L XX 3X(XVPXYQ) 

~MASSXHO+2XMASS*XGxQO Deno S*YGXPO+RHO/2XL XxGx2xxXQQxQ0. 


AY DRX UO RR, xt *XS%CYPXPO+YRERO)+RHO/2*L xX2*CYVRV0E. 
2*XYDRXU0OXDR) 

MASS EPO one GxQO#RHD/ 2x1 xxexcYvxUsYyuxuo) 

MASS*PO+ ASK Oe aoe LYWPXP0+YNR*RO)+RHO/ 21 xx2xYVHxVO 
RHOP DHL xo aoe yo gOteRMASSXYGXPO+RHO/ ORL xeq Kray uM 
RHO/2*L ¥*3x*(YPXUQF YHPXWO) 

RHO? 21 oo gay nA SSXZGHRO+RHO/ 2xL x6 x (YPQXPO4YQRKRO) Tieics 
RHO/2XL XX 3xYVQXVO 

RHO? Ox Lox oe Cn Y OXRO “MAS SXZGXQ0+RHO/2xL ¥xGxYORKOD tec 
RHO/2XLXX3X(YRXUD + YWR¥WO) 

CHEIGHT - BOY )XCOS( THETAO)XCOS(PHIO) 

~CHEIGHT - BOY XSINCTHETAO)*SINCPHIO) 


F2xUOSZD Re ox bab See Ox9O4 RHO/ 2x1 xx2%(ZWKHO+2xUOxZDSxDS_ 


G4 


Plies a DSL Al 


RHO/ 2XL XS XZVRXVO 
AC3,10)= -(NElCHI Ss BOY )XCOS( THETAO)XSINCPHIO) 
AC3,11)= —CHELGHT Ss BOY )XSINCTHETAO)XCOS(PHIO >} 


ACG51) = MASS e000 MASSX®ZGXRO + RHO/S2XLXXGXCKPXPO + --. 

toy ORO/ 2XLXXSXCKVXV042XU0X(KDB/2XDBP-KDB/ 2XDBS 22% -- 
RHO/ 2XLXX3XUOXKPROPT RHO/ 2XLXX4XKPNXPOXEPS 

ACG,2) = “MASS *1e-Ue RHO/2XLXXG¥XKVQXQO + RHO/ 2XLXXOXCKVXUO... 


+ KVHXWO) 

ACG,3) = “MASSXZGXPO +t RHO/2XLXXGXCKWPXPO + KHRXRO) + 
RHO/2XL XX SXKVNXVO 

CG, G) = aA Os IXZXQ0 - MASSXYGXVO — MASSXZGXWO + 
RHO/Z2XLXXSXKPQKQO + RHO/ 2XLXXGXCKPXUOFKHP XO D 

ACG;5) = = F24R0ee LYX¥RO + 2X1YZXQO + TXZAr Us MASSXYGXUO +... 
RHO/Z2XLXXSXCKPQXPO + KQRXRO) + RHO’ 2XLXXGXKVQXVO 

ACG,6) =elc7C0e LYX¥Q0 - 2x1YZXRO + MASSXZGXUO + 


RHO/2XLXX5¥*KQRXQO + RHO/ 2¥LXXGXCKRXUOF+KWREWO D 
ACG,10)=  CYGXHEIGHT-Y BXBOY )¥COS(THETAO)XSINCPHID) «= 
- CZGXEIGHT -ZBXBOY )XCOS(THETAO)*COSCPHID? 
ACG; = - CYGXHIEIGHT-Y BXBOY )XSINCTHETAO)*COSCPHID). « « 
+ CZGXWEIGHT-ZBXBOY ) XSIN( THETA) XSINCPHIO) 


AC5,1) = -MASSXXGXQO + RHO/Z2XLXXGXMQXQD + RHO/ZXLXXSXMWXWO +... 


RHO/ 2¥L XX 3XUOX( MDSXDS+MDB/ 2X DBP ) + RHO/2XLXXGXMQNXQOX..«. 


ell ae hes RHO/2XLXXSXCMWNXHO + OXMDSNXUOXDS)XEPS+t..- 
RHO/2¥L XX 3XUOXMDB/2XDBS 


AC5,2) = MASoe.C~GUe MASSXZGXRO + RHO/2XLXXGXCMVPXPO + 

MVR¥RO)D + RHOXLXX3*MVVXVO 
AC5,3) = -MASSXZGXQ0 + RHO/2XLXX3XMNXU0 + RHO/ 2XL XX SXMWNXUOXEPS 
AC5,G) = =1lKenoes IZXRO - IYZXQO - 2XxIXZxPO + MASSXXGXVO + 


RHO/ 2XLX¥5XC2*XMPPXPO + MPRXRO) + RHO/2¥LXXGXMVPXVO 
AC5,5) = IXYXRO -I1YZXPO0 - MASSXXGXUD -MASSXZGXWO + RHO/2X... 
L¥XGXMQXUD + RHO/ 2XLX¥X4XMQNXUOXEPS 


AC5,6) = ~IX¥PO Ft IZXPO + IXYXQO + 2X1 XZERU Tt MASSXZGXVO +... 
A DXL XX 5X (MPRXPO+2¥MRRXRO )+RHO/ 2XLXX4 XMVRAVO 
AC5,10)=  YGXMEIGHT -XBXBOY )XCOS(THETAO)*SINCPHIO 
BCS; = CSOXHEIGHT -XBXBOY )¥SINCTHETAO )*COSCPHIO) = 
(7GXWEIGHT-ZBXBOY )XCOS( THETAD) 
¥ 
KO651)- > -MASS¥XGXRO + RHO/ 2XLXXGXCNPXPO +NRXRO) + RHOZ2X...- 
1 xx 3X CNV XV 0+ 2XNDRXU OX DR) +RHOXL XX3*U0XNERDE 
AC6,2) = -MASSXYGXRO + RHO/ 2XL XXGXNVQXQO + RHO/2XLXXSKCNVXUOT..- 
NVPIXPIO) 


AC6,3) = MASS¥XGXPO + MASSXYGXQ0 + RHO/ 2XLXX4X CNP XP OFNWRXRO DF. - 
RHO/ 2XL XX3 XNVWXVO 


A654) = =e oe IXX¥Q0 + 2xXIXYXPO +1YZXRO + MASSXXGXWO+...- 
RHO/2XLXXS¥NPQHQOD + RHO/ 2XL¥X4X (NPXUO+NHP XO D 
AC6,5) = =e IXXPO - 2XIXYXQO — IXZX¥RO + MASSXYGXWO+... 


RHO/ 2XLX¥ 5X (NP QXPOFNQRERO} + RHO/2Z2XLXXGXNVQXVO 
AC6,6) = IYZXPO -~1XZXQ0 - MASSXXGXUD -MASSXYGXVO + ... 
RHO/Z2XLXXSENQRXQOD + RHO/ 2XL¥X4% CNRRUO +NWRXWO) 
PV GXHEIGHT -X BX BOY ) ¥COS( THETAO) XCOS (PHI? 
C OXGXHEIGHT -XBXBOY ) ¥SIN( THETAO ) XSINCPHIO) + ee 
(YGXWEIGHT-YBXBOY )XCOS(THETAO) 


B2 


K OK OK 


Np pA Je 
NG Ze 
ACEI = 
ICR 7a)) es 
Gees) = 
Ce Ope 
NGS = 
ACS,12)= 
AC9,1) = 
ACO aoe = 
Gems a= 
AC9,10)= 
CO ee 
AC10,4) 

AC10,5) 

AC10,6) 

AC10,10) 
AG, 11) 
AC11,5) 

Ral tho) 

AC11,10) 
AC12,5) 

A(12,6) 

AC12,10) 
A(12,11) 


1 


Wow UW ou 


EHSCPSLO)XCOSCTHETAG)D 

COSCESIU sli Cine woeSstNCPHIO) = SINCPSI0)*XCOSC(PHIO) 
COS(PSI 0) x Smee THETAO)*XCOSC(PHIO) + SINC PSIO)*XSINCPHIO) 
VOXCOS(PSIO)XSINCTHETAO)XCOSCPHIO) + VOXSINCPSIODX... 
SINCPHIO) - WOXCOSCPSIOJXSINCTHETAOJXSINCPHIO)D + 
HOxSTWCPs 10 2xCOSCPH10) 


-UO0XCOS(PSIO)XSINCTHETAO) + VOXCOS(PSIO)XCOSCTHETAO)X.. 


SINCPHIO) + WOXCOSCPSIO)*XCOSCTHETA0)*COSCPHIO) 
-UOXSINCPSIO)XCOSCTHETAO)} - VOXSINCPSIO)XSINCTHETAO)*. 
SINCPHIO) - VOXCOSCPSIO)XCOSCPHIO) ~- WOXSINCPSIO)X... 
SINCTHETAO)XSINCPHIO) + WOXCOSCPSIO)XSINCPHIO) 


SINCPSIO)XCOSCTHETAO) 

Silcrsl0)<Siieppernuy~slimcrnio) + COSCPSI0)xCOSCPHIO) 
STHGESTOU<stn The ino *eUS(rnlO) = COSCPSIO)XSINCPHIO) 
VOXSINCPSIO)XSINCTHETAO)XCOSCPHIO) - VOXCOSC(PSIO)X... 
SINCPHIO) - WOXSINCPSIOJXSINCTHETAOJXSINCPHIO) - 


HWOxXCOSCPS1O>*<COS CP mI OD 


SUPA SINCr STOO x SINC ReIAG) + VOXSINCPSIO)XCOS(CJTHETA0O)*.. 


SPnCE HO) Se lioxstNCe She) xeOSCRHETAO)XCOSCPHIO) 


UO COC Polo) CUlC i hers 00e st VOXCOS(PSIODXSINCTHETAO)DX... 


SCr hl) Bea Ucn oo ACOsSCrRI1O) + WOXCOSCPSIO)X... 
SEC HETAQ*COSCPHIO)Y + WOXSTHCPSIO)XSINCPHIO)D 


-SINCTHETAO) 

COSCTHETAOIXSINCPRIO) 

COSC THETAO)XCOS(PHIO) 
VOXCOSCTHETAO)XCOS(CPHIO)J-WOXCOS(THETAOIXSINCPHIO)D 
Seon C OSC tne ln Oey OA SPN TmeVAOxSINCPHIO) -... 
WOXSINCTHETAO)*XCOSCPHIO) 


0 
SINCPHIO)XTANCTHETAO) 
COSC PHIO)XTANCTHETAO) 
QOXCOSCPHIOIXTANCTHETAO) - ROXSINCPHIO)X*TANCTHETAO) 
WOZSTNCPRIO)D7COS(THETADJX] .O7COS(THETAO) + ... 
poz GUS (PHI CUZCOUSCIHETA0D*1]. 07COS(THETAD) 


CUSsiGriat 0) 
= Stir. 0.) 
sQueolHiCenl oO) = ROXCOSCPHIO) 


Seen lL OZ COSC THETAOD 

COSC PHIO)/COS(THETAQ) 

WO7 CUS te ALO) /COSCINETAD) =ROASINCPHIO)/COSCTHETAD) 
QOXSINCPHIOD/COSCTHETAO)XTANCTHETAO) + ... 
ROXCOSCPHIOJ/COSCTHETA0) XTANC THETAO) 


WRITEC1O,200) 0CACT,J3,J=1,12),1=1,12) 


KK MK 


KK OK OK OOK 


CALCULATE THE B MATRIX 


Nee’ 
‘ 


= RHO/2XL¥¥3XXRDRXUOXRO+RHO/ 2XL XX2X( XRDRXUOXVO+U0XX2X... 
2XXDRDRXDR) 

UOXQOXXQDB/2 + UOXWNOXXWDB/2 + UOXX2XXDBDBXDBS 
UOXQOXXQDB/2 + UOXWOXXWDB/2 + U0OXX2XXDBDBXDBP 
UOXQOXXQDS + UOXWOXXWDS +U0XX2X2XXDSDSXDS+RHO/2XLXX3X. 
XQDSNXUOXQOXEPS + RHO/2XLXX2XCXWDSNXUOXWO + 2XXDSDSNX... 
UOXX2XDS)XEPS 

RHO/2*L*¥X2*X0.12XCD0X0.12*XRPM 

Sih CLHERAOD 


RHO/2¥L XX2XYDRXUOXX2 
“~COSCTHETAQ) AS TG: at Up 


U0XX2XZDB/2XRHO/2¥L XX2 
UOXX2*XZDB/2XRHO/2XL XX2 
UOXX2XZDSXRHO/2XLXX2 + RHO/2XLXX2XZDSNXUOXX2ZXEPS 
=COSCTHETAUD COSTE GD 


ww wy 
1 onto 


oN oN No 
woot oot 


oN oN TN EN 


Orn Or in tin int DOD NON GI 


) ) “ “ 


a _ 


-~RHO/2XLXX3XU0OXX2XKDB/2 
RHO/2X¥LXX3RUOXX2KKDB/2 
-YBXCOS(THETAO)XCOSCPHIO) + ZBXCOS(THETADIXSINGP Ris 


RHO/72XL XXSXUOXX2XMDB/2 
RHO/2XLXX3XUOXX2XMDB/2 

RHO/2XL XX 3XCUOXX2XMDS+MDSNXUOXX2XEPS) 
XBXCOSCTHETAO)XCOSCPHIO) + ZBXSINCTHETAO) 


Ov Gun) On ED WGP) ne NWN 


oN INOS 
a 
Won oa 


~~ . “e 


ou TN IN TN 
. ww Ww w 
Nee Nee Ned ee 
ob om 


On OV GPO 


Ne VY 


RHO/2XL¥X3XNDRXUOXX2 
-XBXCOSCTHETAO)XSINCPHIO) = YBASINCTHETRAGD 


FORMULATE THE A AND B MATRIX FOR STATE SPACE REPRESENTATION 
MULTIPLY MMINV AND DF/DX 


DOVS0 1 = 156 
DO 70 J = 1,6 


tw & www bd) mw orReshecles) lose) wo 


oN oN 
a 


“~ i“ 


BZ 





EAU) elem 6 DSL Al 


Coo oo 


KOK OK OK KOON ON 


2U 


G0 
50 


KK OK OK KN mK OK U1 ON 
m— © © 


K Kee oO 
Ooo 


KOK WN 


405 
401 


SUM = 0.0 
DO 60 K = 1,6 
SUM = SUM + MMINVCI,KIXACK, J) 
SON) TNUE 
AACI,J) = SUM 
CONTINUE 
CONTINUE 


ie oy Sli AND SDE7 DZ 


DOT50 1 ="1,6 
DO 40 J = 7,12 
SHU leet 
DO 30 K = 1,6 
Saia- SUM + NMINVCI, KI XACK,J) 
CONTINUE 
Peto) = SUI} 
CONTINUE 
COnPrivie 


BOsecoy= 1, 

AACI,J) =A 

CONTINUE 
CONTINUE 


WRITECIO, 200) (CAACI,J),J=1,12),1=1,12) 
FORMAT 6£12.49) 


MUEOE EY MMInVY AND DF/DU 
DO 110 I =1 


DO 100 J 


6 
1,6 
+ MMINVCI,K)XBCK, J) 
CONTINUE 

Peel) =) SUM 


CONTINUE 
CONTINUE 


Hike C. 9 
FORMAT (C6 


m~.~ 


Epo ec 


END IF 
x 

CALL DADSCINFO,ISTRAT,IOPT,IONED, IPRINT, IGRAD,NDV,NCON, DX, .. 

VLB,VUB,OBJ,GN,IDG,NGT,IC,DF,AWN,NRA,NCOLA,WK,NRWK, . 
IWK, NRIWK) 

IF CINFO.EQ. 0) DELPRT=0.2 
x 
DERIVATIVE 
NOSORT 
x 
x 

LATYAN = 0.0 

NORPIT = 0.0 
x 
KRXRXKXXLINEAR MODEL % XH HH HK HHI EK KK HHH HK HHH KH HK HH HH HK HHH HH HHH HH HK HHH KH KK KKK KK 
¥ 
Xx 
. CALCULATE BBXU PART OF XDOT = AAXX + BBXU 
* 

Oo) LOM a 48 

SUM = (0a0 
DO Sak etre 
SUM = SUM + BBCJ,K)XUMOD(K) 

15 CONTINUE 


XDOTUCJ) = SUM 
D8) CONTINUE 
xX CALCULATE AAX®X 
DOS Zr = eZ 
SUM = 0.0 
DOR 2 oe ea ee 
SUM = SUM + AACS, KI¥XCK) 
25 CONTINUE 
XDOTXCJ) = SUM 
Zu CORTINUE 
x. ~CALCULATE 2DOTL- 
DO: Sled = 16 
ADOLCII> =" XDOIXCJ)> + XDO LG 
il CONTINUE 
DO™S3 05 = 7712 
XDOTCJ) = XDOTXCJ) 
35 CONTINUE 


AAXX + BBXU 


x 
UDOT = DGG) 
YDOTM. = XDOT C2) 
WDOTM = XDOTC3) 
PROUM <= xpos) 
QDOTM = XDOTCS5) 
RDOTM = XDOTC6) 
XDOLM = XDGi C7) 
YDOTM = XDOTC8) 
ZDOTM = XDOTC9) 
PHMDOT= XDOTC10) 
THEDMD= XDOTCLID 
PSMDOT= XDOTC12) 


x WRITECS,600) 
X INTEGRATE XDOT TO GET THE STATE VECI GR. 


UM =IHTGRLC6 
VM= INTGRLCO 
WM= INTGRLCO 
PM= INTGRLCO 
QM= INTGRLCO 
RM= INTGRLCO 
XPOSM = INTGRLCO. 
Mees = —TetGRECO.0, YDOEM) 
Ze0oM = iGR eC .0, Z2D0TM) 
Fel = TNTGRECO. 0, PHDOT) 
THETAM = INTGRL(CO.0, THETMD) 
INTGRD = CUMXUM+VMXVM+WMXHM+PMXPM+QMXQM+RMXRM+... 
esa xeUolime 1h Oslti=y OR DIAC YTPOSM=VYORDJ+... 
(ARCs UR DUEED SCcEUsMaORDDEL J+ RMIMAPHIM+... 
Ciel eae oan oui i DoMeaDoM+DBSMXDBSM)+... 
(DBPMXDBPM)+( DRMXDRM) 
OBJ] = INTGRL(O.,€0.5)XINTGRD) 
OBS = OBJ] 
Polen onlC 0.0; 2 PSMDOT) 


UDOTM) 
VDOTM) 
KDOTM) 
PDOTM) 
QDOTM) 


0, 
0, 
> RDOTM) 


0 
a0 
0 
20 
L 0, XDOTM) 


XC1) = UM 

X€C2) = VM 

X03) = WM 

XCG) = Pt 

AC) - 70h 

X(€6) = RM 

X€C7) = XPOSM 

X€8) = YPOSM 

X09) = ZPOSM 
KOs LM 

X€11) = THETAM 

AGE Zs =F SIM 
Z0ERitis-@2ORD = X(9) 
THMANG = X€11)%57.3 
UMODC1)=DRM 

UMODC2Z) = DBSM 
UMODC3) =DBPM 
UMOD(CG) = DSM 
UMODC5) = DRPM 
UMOD(C6) = DBOY 


PHANG=PHIMN/0.0174532925 
THMANG=THETAM/0.0174532925 
PSMANG= PSIMZ 0.017494532925 
PITCHH=THMANG 


C7 


x 
HK KKKKKCONTROL LAW XX KH KKK KH HK KK HK HK HK KK KK HK HK KK KK KK KK KKK KK 


X 


DBS = UMOD( 2) 
DBP = UMOD( 3) 
Ds = UNGDCG) 
DR = UMOD(1) 
* DBS = -(GKKC2,1)*U + GKKC2,2)*V + GKKC2,3)*%H + GRKCZ, 4) XP 
* GKK(C2,5)*Q + GKKC2Z2,6)3X%R + GKKC2Z,7)*¥XPOS + GKKC 2,8 JX Yeo eee 
~ GKKC2,9)*ZPOS + GKKC2Z,1lOJXPHI + GKKCZ, LIJXTHETA + ee 
x GKKC2,12)*PSI + GKKC2,13)*XNM + GKKCZ,149)*OM + GKKC 2, 1530 
* ZPOSM + GKK(C2,16)*THETAM + GKK(2,17)XUMOD(2) + GKKC(2, 18)X... 
* UMOD(3) + GKKC2,19)XUMODCS))> 
me DBP = -(GKKC3,1)XU + GKKC3,2)*V + GKKC3S,3)%W + GKKC3,49)%P eee 
* GKKC3,5)*Q + GKKC3,6)*¥R + GKKCS,7)%XPOS + GKKC3, 8) XX) POs 
¥ GKK(C3,9)¥ZPOS + GKKC3,10)*%PHI + GKKCS;1IIXTRETA tee 
x GKKC3,12)¥PSI + GKKC35,13)*NM + GKKC3,149)*XQM + GKKCS) 050 
x ZPOSM + GKKC3,16)XTHETAM + GKKC3,17)*UMOD(2Z) + GKKCS 5 eee 
x UMODC3) + GKKC3,19)XUMODCG)) 
bs DS = -CGKKC4G,1)*U + GKKCG,2)*V + GKKC4,3)*H + GKKCG, 9)XP eee 
x GKKCG,5)%Q + GKKCG,6)*¥R + GKKCG,7)X*XPOS + GKK(G, 8)XYP Gee 
x GKKCG,9)XZPOS + GKKCG,1O)*PHI + GKK(G, LI DxXTHE) A) eee 
x GKKCG,12)*PSI + GKKCG,13)*0WM + GKKC4,149)*%OM + GKKCG; 50 
> ZPOSM + GKKCG,16)*THETAM + GKK(4,17)*XUMOD(2) + GKKCS> begeee 
¥ UMODCS) + GRKCG, 19) xUNODCGD 
* PUT 20H STERN AND BOW PLANE STGESs 
4 
* ITFCABSCDBS IG! 1.026)" THEN 
* DBS = 0.6XABSCDBS)/DBS 
* ENDIF 
* TPCABSCDEP) G1 2026087 HEN 
¥ DBP = 0.6XABSCDBP)/DBP 
x ENDIF 
x TFCABSCDSI761 70.605 THEN 
* DS* = 036XABSCDS)/ 0s 
* ENDIF 
x 
¥¥XXXXNON-LINEAR MODEL XX XXX KK KKK KK HK HK HK HK HK HK HHH HH HK KK KK KKK KKK 
* 
* PROPULSION MODEL 
x 
x 
SIGNU = 1.0 
TEoCU. ET ot ate Ss tGhu =< —1 50 
TP CARES CUD sia ES?) Ul Sees 
SIGNH = 1.0 
TP CREM LP 20309" SIGH Y= =—1.0 
ETA = 0 01 2xRPizU0 





Pubes PA DSt Al 


RE = USLZHU 

C0 meso 5+ Cle cIoe- me — 1.2? )X*2 

CT = 0.008XLX¥X2XETAXABSCETAIZCAD) 

Cll =s 0 00S *Exx27CA0) 

ee oem OF SN OHN7 SI GHUXC SORT COl+] 0-1-0007 CSGRICCT14+1.0)-1.90) 
YEO we DUxX(C ETAXABSCETA) — 1,0) 


CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER THE VEHICLE 
INTEGRATE USING A 4 TERM GAUSS QUADUTURE 


OK OOK OOK OK OOK OOK OOK 


LATYAH! 
NORPIT 
DO 500 K 4 

UCFCK) = SQRTCCV+GSGCKIXRELIXX2 + CWN-GGCK) XQXL )XX2) 

PeCveCr Choe GT.1E-10) THEN 

eR nOe = CRHO/7Z)XCCDY ARH CK XCVEGGCKIARAL JxXX2 +... 

CDZXBRCKIXCH-GS4 CK) XQXL) XX2) 

TERM TERMOXCV+G4 (CK) XRXLI/ZUCFCK) 

ek M2 TERMOXCH-G4 (CK) XQXL)/UCFCK) 

LATYAW LATYAW + TERMI XGKSCK)XL 

Re NORPIT + TERM2XGK4(K)XL 

END I 


CONTINUE 


ioe 
— > © 


© 
© 


FORCE EQUATIONS 


LONGITUDINAL FORCE 


DK OK OOK OOK OOK OOK OK On 


FPC1) = MASSXVXR - MASSXWXQ + MASSXXGXQXX2 + MASSXXGXRXX2-... 
MASSXYGXPXQ - MASSXZGXPXR + CRHO/2Z)XLXXGXCXPPXPXX2 +... 


XQQXQKX2 + XRRXRXX2 + XPRXPXR) +CRHO/ZIKXLXXSXCKXWQKHXQ +... 


XVPXVXP+XVRAVXRAUKQKXCXQDSXDS+XQDB/2XDBPI+XRDRKXUXRXDR)+... 
CRHO/ 2) XLXX2KXCXVVKVKRR?S + XWIWXHXX2 + XVDRXUXVXDR + UXWX... 
CXHDSXDS+XWDB/ 2X DBP )+UXX2X (XDSDSXDSXX2+XDBDB/2XDBPXX2+... 
XDRDRXDRXX2))-CWEIGHT -BOY)XSINCTHETA) + CRHO/2)XLXX3xX 


X¥QDSHXUXQ*DSXEPS+(RHO/ 2) XL ¥X2% ( XWDSNXUXHXDS+XDSDSNXUXX2X. . 


DSXX2)XEPS +CRHO/2)XLXX2XUXX2XXPROP+RHO/2XL XX 3SXUXQX 
XQDB/2XDBS +RHO/2XLXX2XUXX2XXDBDB/2XDBSXX2+ 
RHO/2XLXX2XXWDB/2XDBSXUXW 


x LATERAL FORCE 
x 


FPC2) = -MASSXUXR + MASSXXGXPXQ + MASSXYGXRXX2 -— MASSXZGXQXR +... 
CRHO/Z2)XLXXGKCYPQXPXQ + YQRXQXRI+CRHO/2IXLXXSXCYPXUXP +... 


YRXUXR + YVQXVXQ + YHPXWXP + YWRXWXR) + CRHO/2)*LXX2KX... 
CYVXUXV + YVWXVXH +YDRXUXX2XDR) -LATYAW +CWEIGHT-BOY)X... 
COSCTHETA)XSINCPHI) 


69 


mK OK OOK OK OK OK OK K OK OK x MK K K OK K OK K 


KK KK KD 


NORMAL FORCE 


FPC3) = MASSXUXQ - MASSXVXP - MASSXXGXPXR - MASSXYGXQXR =] 
MASSXZGXPXX2 + MASSXZGXQXX2 + (RHO/2)¥L¥XGX(ZPPXPXX2 +. 
ZPRAPXR + ZRRXRX¥2) + CRHO/2Z)¥XLXX3X( ZQXUXQ + ZVPXVXP +, 
ZVRXVXR) +CRHO/2)XL**2X(ZWXUXH + ZVVXVXX2 + UXX2X(ZDSX... 


DS+ZDB/2*XDBP) )-NORPIT+(CWEIGHT-BOY )¥COS( THETA) *COS( PH ieee 


CRHO/2)XLXX3XZQNXUXQKEPS +CRHO/2)XL¥X2X( ZWNXUXW +ZDSNX... 
UXX2XDS)XEPS+ RHO/2XLXX2KUXX2XZDB/2XDBS 


ROLL FORCE 


FPCG) = -IZRQER +IYXQHR -IXYXPHR +1 YZRQRX2 -IYZERXX2 +1 X2RP ee 


MASSXYGRUXQ -MASSXYGXVXP -MASSXZGXWXP+(RHO/2 )XLXX5SXCKPQX. 


PXQ + KQRXQXR) +(RHO/2)XLXXGX(KPXUXP +KRXUXR + KVQXVXQ +... 


KWPXWXP + KWRXWXR) +CRHO/2)*XLXX3XCKVXUXV + KVWXVXHD) + 
CYGXHEIGHT - YBXBOY)*XCOSCTHETA)XCOSC( PHI) - (ZGXWETGHIS 


ZBXBOY)XCOS(THETA)XSINC(PHI) + (RHO”2)XLXXGXKPNXUXPXEPS +... 


CRHO/2) XL XX3XURX2KXKPROP +MASSXZGRUXR+ 
RHO/2XL XX3XUXX2XCKDB/2XDBP-KDB/2XDBS) 
PIich GUREE 


FPC5) = -IXXPXR +IZXPXR +IXYXQRR -IYZXPXQ -IXZRPRX2 +IXZXR ee 


MASS*XXGRUXQ + MASS*XGXVX¥P + MASSXZGXVXR - MASSXZGXWXQ +.. 


CRHO/2)*L¥*¥5X(MPPXPXX2 +MPRXPXR +MRR¥RXX2)+CRHO/2 XL RXGX... 


CMQxXUXQ + MVPXVXP + MVRXVXR) + CRHO/2)XLXX3XCMWXUXW + 
MVVXVXX2+tUXX?2XCMDSXDS+MDB/2XDBP))+ NORPIT ~ XGA 
XBXBOY)*XCOSCTHETA)XCOSCPHI) 
CRHO/2)XLXX3S*CMHNXUXWEMDSNXUXX2XDS ) XEPS+ RHO/2XL¥X3x. 
UXX2XMDB/ 2XDSS—-( ZGXNEIGHT -ZBXBOY ) <SUhiGi Be tap) 


YAW FORCE 


FP(C6) = -IYXP*¥Q +IXXPXQ +IXYXPXX2 -IXYX¥QXX2 +IYZXPXR -IXZXQKR ——- 
4ASS*¥XGRXUXR + MASS*®XG¥WXP - MASS*YGXVX¥R + MASSXYGREWNXQ +... 
(RHO/S2)*L¥X¥5S¥(NPQEPXQ + NQRXQ¥R) +CRHO/2)¥L¥XGXCNPXUXP+... 
NRXUXR + NVQXVXQ +NHPXWXP + NWRX¥WXR) +CRHO/2)*¥L XX3XOCNVX... 


UXV + NVWXVXW + NDRXUXX2XDR) - LATYAW + CXGXWEIGHT - 
XBXBOY)XCOS(THETA)XSINCPHI)+(CYGAWNEIGHT )XSINC TREt Agee 
+CRHO/2) XL ¥X3XUXX2XNPROP-YBXBOYXSINC THETA) 


PECZ LEQ S00 THEN 
WRITE (€S;,500) (EPC I), tie 
Z = 90.0 

END IF 


NOW COMPUTE THE FCl-6) FUNG Ons 


DO 600 J = 1,6 

FCs) = 020 
DO 600 K = 1,6 

FCJ) = MMINVOJS, KOXEPCK) al® 
CONTINUE 


THE LAST SIX EQUATIONS COME FROM THE KINEMATIC REEATMONG 
FIRST SET THE DRIET CURRENTS eGe. 
UCO 


VCO 
WCO 


0 
OF 
0 


ooo 


90 





KK OK 


fo) 
Oo 


«KK OK OK 


Piensa OSITION RATES F€7=9) 


poem seCOs UXCOSCPSIIXCOSCIHEI AD 
SeICeHIy — SINCRS)Se0strH! ) 
CeeCPHI) + STNCPSI esr At D 


Peeve CeO nol zoliCiHETADX.... 
, fe CCU Grom oliCTHETAIX, .. 


meee eOes UxStTHCPSI)DACOSCI MEDIA) + VXCSINCPSIIXSINCTHETA)X... 
Cen et esr Sle Os nme NAC STNGPSIDXSINCTHETADX... 
CUS Cri ear CUS CP S4 eS UNCED 


ame seule UXSINCTHETA) +VACOSCIHETAIXSINCPHI) +WXCOSCTHETA)®... 


CUSCPHI) 
EVIDERSANGEE RATES FC10-12) 
FC10) = P + QXSINCPHIIXTANCTHETA) + RXCOSCPHIJXTANCTHETA) 
BCllo P= s07C0sCP Hl) = R*XSINCPHI) 
Gua -sOxSTHCPAII/ZCOSCIHETA) + RXCOSCPHID/COSCTHETA) 


Pewee owt DOHRITE €9,500)0F( 1), I = 1,12) 
FORNAT(C6E12.49) 
Z=Z%+ 1] 


.0,VDOT) 
.0, WIDOT) 
INTGRL(0.0,PDOT) 
INTGRL(0.0,QDOT) 
R INTGRL(O0.0,RDOT) 
XPOS = INTGRL(0.0,XDOTA) 
YPOS = INTGRL(0.0,YDOT) 
ZE0S = INTIGRL(O.0,ZDOT) 


l 
] 
] 
6.00 VOT 
0 
0 
0 


= 
nun wus 

— 

= 

@ 

70 


( 
PHI = INTGRL(0.0,PHIDOT) 
THETA = INTGRL(0.0,THETAD) 
PSI = INTGRL(0.0,PSIDOT) 
ZWEW = -ZPOS 
PHIANG = PHI/0.0174532925 
THEANG = THETA/70.0174532925 
PSIANG = PSI/70.01745352925 


2 


DYNAMIC 


KK OK K 


KK MK OOK »*K mK KOK OK MK 


mm 


KK KK OK 


RN=TIME/ CF INTIMNZ10.-—DELTZ10G0inee 
PNSTIME/ CEFINTIMZ20 .-DELT7 LOCaGe 
O=INTCRND+1 

PP=INTCPND+1 

IPCPP. CE 200 e= au 

1TECO. EO Tig ao=10 


ADDITIONALLY THE PLANES SHOULD BE AT EQUILIBRIUM SO THE 
VEHICLE WILL PROCEED AT THIS NEW DEPTH WITHIN SOME TOLERANCE 


DSM=DX(0) 

DBSM=DXC10+0)3 
DBPM=DX(10+03 
IFCO.GE.10) DSM=0. 
IFCO.GE.10) Deri -6 000 
IFCO.GE.10) DESH =0-000 
DRM=DX(20+PP) 


RPM=DX(30+0) 
CONSTRAINTS FOR A DIVE 


ORDERED DEPTH = ORDDEP 


GNC1) = (CZPOSM-ORDDEP)xX.5 
GNC2) = CORDDEP-ZPOSM)*.5 


AUV'S FINAL STATE MUST BE LEVEL FEIGHIRAS es TULEGhe 


GNC3) = THETAMX10. 
GNCG4) = -THETAMX10. 
GNC5)=CYPOSM-YORD)/.4 
GNC6)=CYORD-YPOSM)7.4 
GNC7) = -ZPOSM 


AVOIDING THE OBSTACLE 


IF (CDIST1.LT.DSAVE1) DSAVE1=DIST1 


IF (DISTV.LT.DSAVE1) DSAVEV=DISTV 


DIST1L=SQRTC CXPOSM-XOBS1)*XCXPOSM-XOBS1)+... 
(ZPOSM-ZOBS1)x*( ZPOSM-ZOBS1 ) 3) 

DISTV=SQRT CCXPOS-XOBS1) XC XPOS-A0BS1 ae 
(ZPOS=ZOBSID*x(ZPOS=Z0BSI Oe 

GUHCS): = GF =]DSAaVveEl) 

NDX=XPOSM/17.425 

NDZ=ZPOSM717.425 

NDT=TIMEX6 .4717.4925 


TERMINAL 


9000 


END 
STOP 


TECINEOCEO 005 THEN 
PRINTX, DSAVE1, DSAVEV 
CONTINUE 

ELSE 

ENDIF 

IFCINFO.EQ9.0)3 CALL ENDJOB 
CALL RERUN 


az 


10. 


REFERENCES 


Rowden, W. H., "Policy Guidance for the NAVSEA 
Integrated Robotics Program," COMNAVESEA Ltr 2269, 
March 1988. 


Levece, J. A., "Positive Future for Battlefield 
Ropettecs =i litary RODOLICS, V.2, no. 7 pp. 3-4, 
April 6, 1988. 


Levece, J. A., "Towtaxi ROV Delivered," Military 
Robotics, v.2, no. 8, p. 7, April 20, 1988. 


Levece, J. A., "Dolphin UUV for Minsweepers," 
Mimbo mR OoOtics avec, now oe pws, March 23, 1988. 


Levece, J. A.,"RECON UUV Reaches New Depths," 
Mallia ee RODOkIGS, V.2,)N@.)9, Dp. 2, .May 4, 1988. 


Levece, J. A., “Unmanned Spaceship to Test Aero- 
SPrualtina, olhibtary RObotICS;, v.2, no. 10, p. 5, 
May 18. 19se. 


Kambhampati, s. and Davis, L. S., "Multiresolution 
Path Planning for Mobile Robots," Computer Vision 

Laboratory Report, University of Maryland, College 
Park, May 1985. 


Wong, E. K. and Fuk, K. S., "Hierachical Orthogonal 
Space Approach to Three-Dimensional Path Planning," 
IEEE Journal of Robotics and Automation, V. RA-2, 
No. 1, March 1986. 


Herman, M., "Fast, Three-Dimensional, Collision- 
Free Motion Planning," Proceedings 1986 IEEE 


International Conference on Robotics and Automation, 


V. 2, pp. 1056-1063, April 1986. 


Singh, S. and Wagh, M. D., "Robot Path Planning Using 
Intersecting Convex Shapes," Proceedings 1986 IEEE 
International Conference on Robotics and Automation, 
V. 3, pp. 1743-1748, April 1986. 


Sh 


i eA L 


er. 


Eee 


14. 


LD 


16. 


Ly. 


18. 


9% 


Kuan, D. T., Zamiska, J. C. and Breeks,) 2. 
"Natural Decomposition of Free Space for Path 


Planning," Proceedings IEEE International Conference 
on Robotics and Automation, pp. 168-173, 1985. 


Khatib, 0O., "Real-Time Obstacle Avoidance for 
Manipulators and Mobile Robots," International 
Journal of Robotics Research, V. 5, No. 1, Spring 
1986. 


Tournassoud, P., "A Strategy for Obstacle Avoidance 
and Its Application to Multi-Robot Systems," 


Proceedings IEEE 1986 Conference on Robotics and 
Automation, V. 2, pp. 1224-1229, April 1986. 


Krogh, B. H., “Guaranteed Steering Control," 


Proceedings of the 1985 American Control Conference, 
V. 2, pp-= 950-955, June 19e58 


Thorpe, C. E., “Path Relaxation: Path Planning for 


a Mobile Robot," CMU Robotics Institute Report CMU- 
RJ-TR-84-5, April 1984. 


Krogh, B. H. and Thorpe, cC. E., “Integrated Path 
Planning and Dynamic Steering Control for Autonomous 
Vehicles," Proceedings 1986 International Conference 
on Robotics and Automation, V. 3, pp. 1664-1669, 
April 1986. 


Gilbert, E. G. and Johnson, D. W, "Distance Functions 
and Their Application to Robot Path Planning in the 
Presence of Obstacles," IEEE Journal of Robotics and 
Automation, V. RA-1, No. 1, March 1985. 


Johnson, D. W. and Gilbert, E. G., "Minimum Time 
Robot Path Planning in the Presence of Obstacles," 


Proceedings of the 24th Conference on Decision and 
Control, V. 3, pp. 1748-1753, December 1985. 


Sanders, D. W., A Feasibility Study in Path Planning 


Application Using Optimization Techniques, Master’s 
Thesis, Naval Postgraduate School, Monterey, 


California, December 1987. 


a¢ 


20. 


ees 


22. 


Vanderplaats, G. N. and Sugimoto, H., ADS Design 
Optimization Program, FORTRAN Source Code, Naval 
Postgraduate School, Monterey, California, June 
1982. 


International Business Machines Program Number, 5798- 
PXJ, Dynamic Simulation Language, June 1984. 


Olson, T. F., Application of Numerical Optimization 


in Modern Control, Master’s Thesis, Naval Post- 


graduate School, Monterey, California, December 1986. 


35 


INITIAL DISTRIBUTION LIST 


Defense Technical Information Center 
Cameron Station 
Alexandria, Virginia 22304-6145 


Library . code ml42 
Naval Postgraduate School 
Monterey, California 93943-5002 


Chairman, Code 69Hy 

Department of Mechanical Engineering 
Naval Postgraduate School 

Monterey, California 93943-5004 


Professor D. L. Smith, Code 69Sm 
Department of Mechanical Engineering 
Naval Postgraduate School 

Monterey, California 93943-5004 


Professor R. McGhee, Code 52Mz 
Computer Science Department 
Naval Postgraduate School 
Monterey, California 93943-5004 


Professor R. Christi, Code 62Cx 
Electrical and Computer Engineering 
Department 

Naval Postgraduate School 

Monterey, California 93943-5004 


Dr. G. N. Vanderplaats 


Engineering Design Optimization, Inc. 


1275 Camino Rio Verde 
Santa Barbara, California 93111 


LCDR David W. Sanders, USN 
204 First Street 
Butler, Pennsylvania 16001 


LCDR T. Olson, USN 


6905 Ashbury Dr. 
Springfield, Virgumalawe22o2 


96 


No. Copies 


10 


10. 


1 


12. 


3. 


14. 


Jaye 


Russ Weneth, Code u25 
Naval Surface Weapons Center 
White Oak, Maryland 20910 


Paul Heckman, 

Head, Underseas AI & Robotics Branch 
Naval Ocean System Center 

San Diego, California 92152 


RADM G. Curtis, USN PMS-350 
Naval Sea Systems Command 
Washington, D. C. 20362-5101 


LT Relle L. Lyman, Jr., USN Code 90G 
Naval Sea Systems Command 
Washington, D. C. 20362-5101 


LT Richard Boncal, USN 
Raymes Neck Road, RFD 2 
York, Maine 03909 


LT Winston Smith 


7979 Cornet Place 
Pensacola, Florida 32514 


od 








ae 





















LA wee. A 
a RAY DARe 
4AE4 OS © HREM UM B. Yodo d Panse . lt 
yee S845 hoon ats tne det . at, * * 
iJ 


wrntiggnt cites SFDF. BUA OSE IM Nae AP 49424.: 9 iin O08) 25 mb. 
nije hl eee nei Ree tt TEE een Titan tetra eas § or, S6092 
ADEE AAA oo Hoh od (REDIBY OD.06 Symi ok eee: ah Cle thes 2 
iE TT OGRA PE 1e BSH > Ane 30 Ft nh haat Roe d ad Sts Aogies oe Wi irslng. £m am tv) yy ae fy 

pap needa dy pel re Les Cad Te nT Peery PONSA) Yoh. RMA Po tater) BAe 
Mapbtthiadiihbddaltel tLe ee CARE thal Ma tl yeah hn Sk HOLT M. b BG Was Wret,.veLsathete 
BUAM GP TO ROL WAS FA, BMA E OY ® Ln) ELF BLA hh oo TOK rvce * : rw 


. P 
cal path planning using optimal contr 
DOBLE PEGS OM in ee Lom one RH wht a3 nian aga ater ‘ahah 2 4 


~ i ' 1 ’ | wel | 
» peeny weet RCN) PUCALIVEDLUUD DEH! | M8) 
i Mor toRt eh ahs BP yp atehe By : i : create ayy tal ] } })} | 
OA Rey PUA BOLE 1B 80: Ron) 5 AGP W? 0 Aremop: Aa e er papa: ee ee Ce a Loh O20 ere 1) | ] | Vil WT 
REDD SERPS? 0h DE Det He oh SAROARLES OEE PY at a G, ©. 0, Aime Oe on 59 5a! 20h A Sarre 4286 ha ea.’ . te tH ce eae “ | | tbl | {1 
Pea chan ae ma enincd ea TMA AAA) 01010. 0cRh LAP sa ealahetrGeh & ane Ue et tS ee Pk te ih) LUBA) RIAL | | 
08.98 poem oP eee BLT 10 88h iA ORL O. RO on ane BYPASS PEROALSS Phone BAS439-4 DA: karnee_e ot iA Seay 5 fee | | 
pte peer pee GORA, PSOEAAM Ame CaBewh Es 0.8 Lickbhnd 3L" won ‘ §. hat, w 90 Palins i WIN VURAL AR | HOt 
AR SAREE OMIA AAG OLED EG EMA 9648) oh of COBO, AFUE ACs 5. x | {| Wt NNN) Walt} 
| ' 
OV EUAASMSMPAOMDUD GML YOY QUIT BUSY BDL WT 
| } | 






























¥ So Mahe BS Oe A? Winall oP > ST 
AYADS pS TOLALD OIF CRE 9 oP ABE MAS Ah Oh tee Demerath Ponds ig Moneaks H. 6.6. neienelen " ity 
WAP AALIGLDIPLEIONS Bais BARU &OSIEAA OA wl Wo 26h Org DS Rare a toabe “9 Beat Ve MiGiha dd dem bdei. nn c 
Peles ape penapetboneul-et ogipumen agit makes DoT oft rpctey oP GRADO ORES Phd LOAF vale «0. no ld obhctsa® er rer 
ORE RAIN OAD IMD. ONO SAA PAGS Ho BL CT ohn rbatt | nLAlO0s in eh Oia Dom AG eas eres rr yr ear tre ( 1 5? 
FO RETL PRON Pest shod wr ae Seat #2 0 VAL gue hs ORL OdE.Ar tel NOE A.cteh et. ADR errnery ire BO Re shape : Fi 68 
oFenp Le ® Bee Ft ofios BYE UE TA AUaNs. sh paand. NG On OM Brora he deems pan po ofa © BA GEeMDPLAR® Pe =2he .b pe Dh =] “ 
BAG MPRA COEF WS AR6, HTL Oe od OF AP ODO), Moh Ah Hansirtte aaapy a Bl efat.@2n.6 ph ss 


AE grehnen er me? ote 










































































































































































































































































































































384 a 
SAPP ed 4 etame Me aten 04) wha gy whis. ee et » - ‘m8 KNOX LIBRARY 
CD e CoP et DUES 849 a Finke of opines 44 Oth ot PAF G8 0 ed B9ESE Boe Ale ns 2% ahisercod oiss DU D ia 
Oe en LP a ee SPF bio G:C wae pad FRO AIA Pontes FS4 0048 18. @ "am ee 3 s -{ 
COOLED BOO WA EDP EB, & BBD AEH MOON @. oT oteed &. SOD Mohs MARA RD see AGL Bie MM 4.8 aga d 
aA ee tent hoduatelah ieee leah alate ata Y The aha dt Sain WET hae st ewus-%as i oe ob van lilies ole. bathe 
iedenentthiengl te ond aim onde ob ede) eh Judes bi devh cel swale ett ak Lee Pe ee A atts. al eels dle th s 
Cid entnehiel eed de ae td Letwiiy ys COM a VADs oh RaaSas? han sng ae eo , 5 mame 4 s bes 49 7 " " 
ORO 6M IGPOO Ba aad anal Oe ON A ROL IMg An 04 Sa Om ae Pd on 299 pn! ti shsP anand.  eihehiel sual bok a) ‘uta Fa P mS Wis 8 4s ’ : ag 
ORE AI RAD M0 ReeAF TC LE_IACOAE SABES AAO G9 6 aOd UOAGALE ut. SRD ODP Ke Ped Bape ROE AD > doareneeine Rane as : % 
1 PP er hehe arae Pye Sars AG GE A oho R made ae Ue Cuma rot .gieBAD HOS thi OL ARADO Liia8180,, mh ne BS how * wa & ' , ‘ © as 
ciel gud teeta all thee badididdaeadds-ublitined. olivia ttt en reeq BRLAGAMIEOMOR.S Barnet, st ahi Mohh thn Nanemstar 04 strech 
WOME OE MS ELEARG Rowe wae OS ofA oP DHHS OM Rah 2 wi AIARIT GRE 6G 58. <1 oe hehe O: y 10. Ue pA * wish pndete a” a3 ‘ ait tat 3 
EPA RANG a WP eH 2H AFORE S of OLY oe Cdudinketl-daldtssteent kha til ot eae ee} Pah 86 Oe RokDA eT mo § tered al Serie » * . 
OA a o® a OV OPM Och 2B CH 4H OE) OKIE RD 6 5 © bd het Otel.» fuge #9 oA Rong. 96 A fir 5. ou0te® Aarokot sols » A a $ 
bphearnpiutbniphacht np oho a) pedehnl nt oases He Ore CATE TT SPryy sar EA ot 2 be hase * ” , “es 7 
Pd athemahsend Ilan hl) ree alt ae, te + Reh Did aginbmed, £46 pecans oh ante BS FF wrt aes > ae ! s 2 : 
A POL BOO: Rang OF On EAB ENO ys OETA Seth Ra nade .d ae Agtt 8.4%: a ah dapat the wie Belew + ae gaa : : 
siete athediichalen edatd Le ee A ae, Clty eer yy pepe re Re ASEM. RP Tal a oe : met c : 
LOS A OH. OS OF MF Bhd EM otet « WA oh Bae. O78 4 OMA E ES 24 Ri Michel yhg R#O, BAG Oe babe at re ee RG El Lg mee ; 
ot etd ee Te Ud 9426 4 at APS PPO RA A Od EER Ed. wo: whG eRe os het ade, GO heh» Gor aby ot a + 
re RIE 6S. ET BY WP BRE ARAN GEMM Emmy 10 G POE OP Ha IES ASB Schall me 94 a 00s omer ag aad Fo ~ 2 sae . 3 z ae 5 . 
gi vir eipta, smug pealeat abe vine tle ep ieereice coiet ey PT Ter a Hs oR oat O9.%. tard te oi U eS : J 
Bc aap 9l ppt spit dopatecth tir laamipauneaien tet ooedbepdee ane eT TAT ae nae oP OD an one oe : f : : 
eee AAO PEDO TARAS OP 08 rAd SLA RASAT" 308 AF 1Q>t-009 CaN fiat om 4 § PdoRatennan® pmb ea dh vf . Se 
peletptventintd ated diet lt ee ee Be Oe ORES Oke Oto DD dow ” « when? Ne @ Ave m4 & st ad La ' © a r r) t 
OO DT e oh Oat wh Oot 7 gh @ pach oho 6 pOBHING Oa, VELBA HO Br Be ae Am Ay =a Aba rz “r ve a) a ap eae . 
OE ORO U BA is aes AGO ha 19 1H PORES: F OM Fat ES aan. hal.» na a Pn i ral atone weet pes e : *¢ . 
SOE AEM BO Se Dag gM ne ft WA IRS #0 oe Oe, sth! phot eg ieg oom 7) fo o4*.@ £oge oom & isd oe a ‘ ' 
QAO U bt oth * ob gO CAME OPES 074 O1RDO OEE > O8 OF UF 8 inn mar am Pigg 0* seat ah a 
ehh lh phage a ee ee Fates Oty Be 46 60) Bo pom. a&.4 e f mail , * 
eS AMT ODF LR IA EFAS O10 8e Bash POR AP rosa MoU I? ErKPSADAR o® neo on 2 He tek ae . ° ' ¢ : : aes 
© PPAR LI PA at 0 tA Ye TAR EDD OES nse: LER BAr ie ongted “aes on *. not foe oa hl * angie - = als 
yh BANAT 9 6 -LAhe 20 Fare eSGDEwTOP> BAENGCLE 21FAD 69052 mtn: SLR of, we AD G9 5% waft ‘ e t.aas 5 
sgihdliedadentaathem ales mended PP ae — pe ® * ’ 
mye NaF ANAK ERAN ORE 0 UROL MON wot wh ted ? ath 32-2 on a , t a ry 
paste Ladbatend pamtiealveh die ace toed ee ee a rene Tame \ TAHA AdAume & r i s 
ulnogtnt idan at deindgade tee otet a el en ne roves LoaSahdet mse bot *a°48 oe ook a 28 fi i 
inate tedad x putehiente,t-samkden te ee 0 ae Ret. 2 Bg wae 8 Qpna®..ase RGh A 0 “hedetne “ ° ° . . 4 t 
ee PE Ate oe RE oA RO MIVA P LS abe hy AY Pee Re pele A ey nthe, 2 0! © Te OF amy ease s 45028 4 ontyol % € sthist.e r 
od AAO we BARS EH) MeBE O6we AFORRL Ae Faw ee, FB Ot of Ef vie Agh age “Wi odor. eomet dus ‘ . Mat s d ‘eI ' 
CUFE (AAEM Oey ERAA nbn eA Oe Ee eOnt wns 105 . * 2 Bet ¢ . ye , 
PAP? COMB gage eseild om a of re eT) , S28 ] > ae P r ' 
FOF De ohn 69k oth mi ea Oe ee eee a. Oe 5 ons 7 mA? 9a aa =) . Y) ® : a 
o ‘st edt a a am ee oe Pe oe om 6.88 ; on ) ¢ 
WF BAO RS Bh awE age need os oer La? ~ Bes Wek emen bas geen he ® saa: 0 “ : 
ADAP EE A osm: wt tal any aging? phiOL) 80H v2". a ar Le eT ae ” seen 8 ¢ a 
OPA DERE CRG, SHIRE LU Ie |e Re oon e saitied a ei ee *masngn Sq" Ae Feo * Sesae . 
eee Re 88 £9 A EER IOUS a NGR dO ~hlvay ant © 2, onahe’ watt €4.90° AO? .: 9" wt one tao ‘ oe a. 
bans sdhaaahearead nates het lh bal ted gate heey CENT * eye 5, ws a ee o Ame Usapates ¢ 0 a8 % 
tO. 8 2g BOOED» POLE 88 etna oe I Ft AAS? nim A2aBet ar ot ot smth. cdot W tah 8 ot hn nant ry &a* 
B08 OEM DA ona of oh. 8 ta FATTamen es PFD o SSO Det R* BB POS 69-8 AS log eT rey | se . ae 
piinthy, tintehatepivatetead mhiat eat ot Leet ne ee ee ot ee ae ot ae - ®. a -0 © aoe 
© pion om BF RT ad Dtonf am Praia iptiend: toalinal ah pill mainte oi peng Le MERA MAA GReP ge we ms A The dee om a a ° 4 ® ® ’ . 
whan bled aahatahbey toed nba teat tanhien ar-opin dts tk he re ae atone ° z s 
Sutiintes Mtl ih-eha lahat hin ot hte D> aed et om g aod p< wo ate Do be 1 , ‘ 
eek el he Othe § ae > o0de Bowe? 256. AtAL a8 » 6 OO Sel oI Ait om 4° as ® : 3 , 
te Tee a? MO ant ron Bergh ON5d wings oO mh dae Py” Ab hh Mel inane * gam oe a * . 
som qi ate 90 OEE NA R™ 08 2 A BABAR OO 0 AF Gat ag egy ai ot prgdes al} od V aP or oun tee Vee ~~“ : ° 5 
oP PAGE O8 AP. OF OPS Cn a OB OG. Be QP, FP at ot traits 2 OMG ter ts og and « TT o ae atm " a ” Site? a oe 
eter BOE OER CUEND Rend > 9 tt OO aD el PAGE Rh i OK Ot re of iy ‘¥ - @ AP ae aw o¢ booed P, 
7202 AN RNY oF ow Olt PER AF $0 MP Bae a ht qs whe beatae? 4, Pov) . ° y ’ s * é » - 
aenen Or BP had hemes re Bot Pod 200 eb enwighhin®iot = 28: spree B ore roms tb oe $4 Ps ; ho s Nay iM 
e NE OIF EG DD eae Oe Or ON ore es OS FARE Fhe ae 9° reans aA a0, @ Fe ? 
Oot OF AS OER Gh PAP aM FORTE AR OR gia hi: ORL AMD ewie ne: s. te (OA rsp mhsae 0g . « ose is : 
. 7 ME Pg an ba iF Pace 1 ow? “ 48 . Pht wo 4 ~ nth rf . 
phathn eeattenen inal he totind detonate | OF BP atone 206 — ll or ey) 8 ’ on ’ * ont o ? 
Nahin anh Wh get thaked -lakot ot a ee wegen hosyh = we w* i * oo ae) scmihee we s : 
‘stmmer 98, PO ne oA Bei ao: gat PP dd WE LPM SBOE me Lod ee — ane of Bu» vf reer r Y Ly 
plied shaven wheel dead ae wcisoas tt oe chyitianmah iat th sete @ Last, MARY Hs me ake i # 
OOO em ONS tse Pee wh hm ahah she tine MoE On pinnae nda daide lanes aha at No at P a ae nf we ° Au $9 
Cet et od At te aor maen Te PF ee lb APB co tm oe ee | a moe ‘ os ge ad LY ® 
C0 Oe et OMA AL ee Ut yi BPD IM thd nad A «bao ce a > eof on 1s \e beet ~ Td 
oo a. ar 28 ane & me ag aime Se 40808 PS Vy odem,?, 48 bated , #*, mop as oon i 
akattidenl hd ant are ae Fat tat ad wet eB tbe 9 vy But bye a ae iu » 2 te 4 2 
ste Alahatradeatataaiel ol dated dd ont © 6 tare te . a , ° * Pi. ° eae Os . Bo 7 
WO we mw ew amt ot AP FE etd Pat meg oo At et owreta ab eign. 2 ke ate ‘ pi ‘ 
Oo ip OD 5 pret Chet be 0 oh grab as ob mmgem ~ o 0i 7m on 2 bdoe " . 
7 Bom water aol. ey Mork tele tae an o P » = » te ° . 
herd: atin eet beet ah tod tae ee ee epee Ppt mehae wt o® 8® } ate ou oF 6 : - : 
“ edad ofall pated dant Of le re Si "2 A* i wos ow ‘ ay LU bd 
OOF S 2 ot oP ane 96 DB Mot a enn wre 2 ome =\2 Be as . = 
a a) che Mare <A%akm dot a> ote? o ene * a,° te 2 
me + Cet arm oe» — Baers J as; ea icean ? eke ot . it at e* oe 4 
Be OF ae AE NO rat Be OND ee it 2? Peet OP ot Fate ws see ww eee 
SO Op eee ww, ais pk s ante es ot ed , rc 4 od mee 7 
6 ab take went FeS a wt ante orn Peo er rere: @ od ipe sdb ¢ ” 0 be "gars PT) . e; ed a 24 e 
es Bm a ete ih > oe  & =r * aw Oo ee Cam = as Shed : sf 
Cee +5 owt Ser ee med *, Cre | LJ sd a 
Si cetel ee gs Do eet a Dang 3 o , 
2 en grat OP ee on ome nF we ty 2d . * 
{an gine ob ew pb - 
fa he Wate le m: : ans 5 aa o. 3 ° 
peer st: re c . e noes eee Boa 
> wow Be boo a ‘ ® ® 
> oi 3 s . * ® s n 
2 AE Fal S.A on Le, aoe s a) 
Et Dense @ wet ry 
"I Fer wont ‘ 
og Mere preety: ¢ t 
eke wr om. . - 
e 
ae a ° ° 
t f : ‘ @ . s 
o ‘ . ® - Son ® e e ‘ 
» 7. 5 
° . B fae s 
" A . 8 * 
‘ s oo? ® Ps 
J F 
= a a 4 . ‘ 
eI . ‘ 
rz q rae ke 
4,735 5 
f tod ® > 
v - 
ef 7 i . 
5 * a 
é 
> 2 é 
° o%, 
i » 
. ° 
e@ 5 ad 
~ ' 
ts 
s 
s * 
- ° oy * . 
we te 
a r 
at @«¢ = 
r4 : 
~ wires. ai00f" 9 S28 9 7 5 os 
Sere are ~ * dgye ® . 
or SAS ieee Senet 48 4 eatin athe * “ wv bs a  ] 
We ie Ty ps MS ee rae or pess ct on ae - ' qe 7 * 7 c 
sibaliauiaitn Pheer ee wes ‘ . - ap BL § e . ba . 
oa i . ~ -* = = = 
Brien Ki ay Soave et Wet} : 2 +e Eom : 
a <8 antes faked aia Septet tad Ge ' 
ad Bs Teg A, ocean teet : * @ Pe 
Fee 8 ect aan 55 ~ 09%} ‘ 
Cm Pht 95 go pees oc ae A eee x on ‘ “a me ¢ . ‘ 
nee VRS neg liiiwsie pub: Bate ms Hyde wee a oP, 2” t “ 
et shies be Baten ee eo eA Be ; a] tf « ’ : 
9 PA Na ah Oo eee Spore vies pe sy wey) 4 ~ ‘ 7 s e 
wa Sola Tepes Wee oe by wey H Somes teen Fg P Q *30? way’, wi °> 
a te behcaeetunlanhe faker te Sto te Fe = we ee ST kt : =A ere) Th +7 ? 
oO Pr VEE pets mae HI DONTE xr nel eT Gufs FO 00 ge Eee. ory Hwy te ooh - « 5 ie 
alan Seibel thatthe welanptehene eee Le Ca pe: aie teeta eee = ee "aly ‘ i i ; 
ov eg aged we Ene FA Screens % wis oe vrata: twiye-9 Se) Prato. gy weg Gogh 5 o¢& > * * ‘ 
. ws Gye Sa) > ae Ye 2k * a a ‘ . ¥e - J : 
=e Ee hea tome , Garciag Sedwcane Wty ee . eye an . % ort hI o! * mn 
SUNT RN STW - REA OE © Jvoitnerg ere: PING sere a) ete WR Ny x nd 
sat Whe snd on toe =o’ Sore WSR RED foi, De wv b= * BE Nee 
oh eteettntnn dee et te why % eo me. ee — & ® 
=p itilanescateeed eed, io, pryenrthe hee ee Fe My ee Ateicte) ey ate 
reer ‘ bn th ie i BL? ote eee wre. of - * hm os : 
Agata ok, rahe + de Lae eles beter tb lelp Mibtgulish Lento bk sate’ aaa + ‘ 
added uo Se ok ee, pry Oy hye ae eq @ : . ou Fi . 
sh edéilidsisiemuham bent ae ae eae Poroed mea Sed oe Dee Lm pe aE ae is tone = ened = “¥ ¥ 
VOCE vey Ov sw Tlws Qe a-H oe Pea eRe ee wud He Ae Owls few i Vink ths ak OL e . “ne ¥ « . 
nae we Uo-horindipy dR rime, bade ed tetas a beeen, ee oF am te Py Cy Ae iw ~ ¢ “= “e+e 
Oey rE sek aikeb be the tnain tie, Cre We ego lay Ub ho mere - . . . 
Se ie ie NLD rar ap here oe wr chiang) © te Cwm a ~=s BER Oye me oe A = e X » 
bcnctibve) ,dinitty-ttastingn dttemtinnh nee en a ee Fe ee Sete eet = 6 1 i ‘ . s& . a ® 
ya intial pigupiainndtitneaea etn tani at ee ORO Pee mei? terres) own cs . ‘ 
Rihxihetedi tet urhhateiman ade ha ee PGW EE EM Sarhe n tnyteaoe~o # Mey Apes ee . ‘ ‘ 
Ral See ng WD eran Kearwh ran owegt rer wrt ere ate 1Eey oO, ow ES VRP OL ap By ay BA me u . 
ban, dedth thet teaetaarmbineent ame anda ed Laphperdallinds bie 2 >itediats intr tn awe eee s ome « 
Se woe pent Pwneg. GER pre popen ety wend dorpndy ey Be iy Dt me Rene Lorde, DEP oe Bu & @ tb ng Cee . 
vubdinttipatetn baa tided ahi ee Seti irewy pelbee ayaew. eve biwie . - ~ 
Aone PS GR Ren gate eat eaten Wate Bly Mpa GDP span Reds BOWDEN CP: OPE, Mageh ® . e 
Dash Minideewhdiinthe tepeittsdhintiodin ths te Mice eT SAP OREMOTEAY, BE SED Lcenrw rie Ba tore it pre lon ty ‘ * 
OR 8 OMe te Fete Ow a 69 Mele Bery ow PEPPERS ew CHT Pratewe My » Wee 4 & el Bere a metas t . . ¥ ee é 
ee ewe te hve ndihin teh beck oe mae Po we apie ae Ua Nig eet ae Dov Wire TI: Neng: er eeee yt ly we al : ® oa 
preshty-aivlajhdinn ppt Marhtaltits ceetteinied teettidbnaue orca or te te SW Aye &. WEE MT. 2 2 ye "ee AEs “wee 2 Ody % pet i a | 
bnlertendiniichbanidemedasiie ae intiiio teen WE Nine 028 Por ei ty PN gw Pote ie ed a] ‘~ "ad 
rR Meera A, chy Mie Cand a9 et Me wag bats teadlint'h. cubdenedaaieh aa ee ee SOs ewe oe Pregl" Pe Boy a ww . a 4 
Cee eer (Weer ee nr eS) Wes Qn ety gta rs NS maine, 7 my = . 
ee Fowl Pete iy t ideat-inndewhdebvenaboier homed ee ee ate bt Pa ay - 
Fre REN Vira GVO hey meee Soutien AS? WOW! Chere 4 Md PEERY 7¢ Ree 4 
re TN, SFR ON OD « oes MBO SS Ade DOr ABE? rergregyn tangy. ght Dae, "Sr, # 
asstnnatanaly aieitndiride Medes tay dk ah eeaee ee hieee he et th he La) * (EOS arte ty - 
9+ @ Cpa TRE Cena serwe Wty te nett teeth thin Li Le eee 4 te ‘ —— ay Cr. 
Renee ee Serr erene ear ENTE w eesias conrcb ears PS eee as bel Te a vin -pmome S.:90y ¥ . ¥ “<~ . 
| apeianpengieerira bear a shee Nrdand tatg Gl et et ate ee vor . " ' = : ’ 
bpdotirtenininds a ewe weet Ve eeu og VEC, a3 s eee | ® 
Saban -tule-sirinteunetad beat beinie ae tr he ne a | rae bes Ge Rhee & 4 nt xp Adri otal. a te "e dis tes F -¥ 2 
RN Wig Md SE Ce eer eRe Sys eens we PS FPR hole, bat tna i dhe bh eed took in 7 “y a°\ hee ale ® . 
Pitt hndatnatteietin Lteticne tee ee EP PTE. re ety sy as ur rere ef wy ¥ ~ ‘ en 
Sorlfhlibsdtiytesinhs, dobdapa eaeieeeh Ueto ne ee ish ob conn Fehen dae TS vee c ” |i . és 
Sati atahite hier th ha We Se ta RO6 Ovo Osea tleyg w genet Mee? Fae ee ae yess py ns 
neh ON are ai 06 ag weet brent figaes oye! WO 6 Gan on wh HON NCP AR 4 Gay Tet Oy me wt eed 
J v 
A © Or eta te ag whew a YP hnsxeth ditbeitn this bt ole “ele Pw vee go ey Et Pore VO ear € e ve ay * 
saves wedubeovens egnanee eee ~ Jane det, SOO GTM CEng TSS SECURED! ge oo ie “iar 1 .¢ : 
ee Sn ee me an P “aed - . 
Pbtretatlpwtgaicdtity tel mares ik he i a enon hae Ae : , fe te: ce ; ‘ % 
OMS OEY wr FRI, Mar eeE Yt OL HES tes ee OPK OF EMOGE LED re OR ey St TESOL Neer tPngy uhedam egy nt hn x oie ’ . i) 
pr epetitabmetebigetnethdinen pireckire echo et ee PHA E  HeNy @ : “ Se : 
hei bette in Pa he t baneted et odathed asst ee NEED UNG) ee WE, 10a OES . +: € * , 
ee OEE: PUT wR, Ww TE oe tind Suthers dnt Wf te dromNy CORE C Grae pis ys eoN run r 
Atheiptntte-tate pad Aedohor hhaaieaebehodikne Leen ee rnb dy <M, (Wwerwo re 4 . x 
echcbdehaty-ddnsleasinatth | te mel CO eos tary ER CIEE wag we DD ne : Wis SO Ord, Peerg © ti5e 2 * =e . 
GN Oy EPG 9 Vey TTR MR ys gp: 2 °PS "9 CHP RRIF Gs NEw Newtbblidted eh Pale Ue w pwn’ * . ’ 
TP SMOG OEY He ay dyn pabhteeh tenis bale lah men Te ee CPS EA 8G Ov Yom rs “ $ | 
Admalprishbiebipehtirdhenietr obit Dh da take PEL 3 t G8 tM geeny vem HEE . % a: 
i tip th Ponind a biabitetianihe er te PUR ONS Peyees® Ox GIT Oy prererre te ® k a ung ay t™% ® ‘ . 
vi Seshhdahiaatptfrap-Liphenididee tak ce ee at nace . OA Beh olen ade bene te tee eee four 4 
BARE EG ETS = BTID ANS ~0 Oe TET IVE Ae OF N00 era tel Se. o a- sereery OE CP were wow e MS 
binhnattteantinirhihpdumien ste tile Uinta tt Wevt rcs “y rEyes Pug wt Pg WI 4 PIES cP * « 
MP PATOUTETUI TONG 0 0 Bad @ gre OTE On gh: o> EYE FOS i POPE SEAT NIE WY 4 APor WEY" 1H CUD 
Pa | Cy Py SOG Pewee pe per Sethitinaidin bene ted ahah ce SUAVE yes, MED reeset ey — Tews ¥ 
n-hatimytrisc be, Naha sep eed donk le shine te WO 8 e © ¢ RP8 hy dma. We wa lerere-s 
vist tehenievind| “aphmcbadthetalmsd ah at tak nek So Py eter ots VOb Hy WT CAO Yer < . 
ORRIN 8 On eee g hutalipharist-fiteultiatetetuadecmel th sa es Wi a ve ' +o Ur 2 yrerw ete “a¥ 
dhtd te hria. ba gpreetin, 4 bite traehe evry ty Herts'are; ok Le Pe, “Sew 
ibn ldelnleahasied init Lk oe te FORO STM Ee. PID. heey dutyt® Or yp + a 
henart tele nh Lnthdndidus sah mode he otelnee PIGS 0-8 NP FOG POV | a . 
HwRe: AN SECU COME os w SIDS EVI R ig Obey Hi Bo : : ; 8 PeTete ror f) e 
WSR? 6 IY WW ON SINE rire y PIVOT eral St be P-GP amEn g & Os, WETy ey er o¢ ] 
yo-ttlelide-h tee, lleddy dha Ieiedto ee 6 SUPE rE LS ’ \ : 
sdbuln ulthtdiedibeldiaha bone dovaea te Phere Ss . 0° Cr Say < *e 
WORT o" 6 Ow BEM EOP eres WES OO Yr we ore 8 
Oo CPUS > CSP Ah wpe De eG AO © 2 ye ae ew We . 
oP etal Wn Rt eC ty THe g 80 N DESTETI) BEPD TORIES "4 ’ 
UPAR be RE teria TRG Oe SEwry orm ves & de Y ’ 
th ire nro worl leads phb=-hebe Sihetedih Wh) deden dtd that thas pee eet e + OGEe oy ty 
Fee WS yey eye? Ldenthaetepetiitn, dhe “adined ten Lado Ba tr tle TOU s0 9 Bw | 4 aad ot Prete’ ( - 
OE A Oe 8 CF ee OR Fy Ee ON Sor) eo ow SPUR" SPW ere ea mule ryt Od ee e 
beltastan drt p-detdiiderttrten te denlieela het cass. se tend-h Meade tick tek) Pe Fy VP. : 
WS EH Oras WY tage PW Gy ree iret hibtrhn nahn a kan ye 20% wey oe ‘ t 
MP0-invisichay-tineidnbedanenpih-t enh babe tet Lek hd ta ee “#50 keene wrwRm % 
PhAvehienthderhinklidytn Td thle hint pdt that snteh Yile cath boun Cia ie voory ‘ LE q ® 
taht deni Pi Reese ore bidutpaah totes Ja thanag mend hae sla ke pee feat ~ 08d i ' ’ 
hdeheeh th) SVOOUTIT Ce OONErE" vevEE Meod eee f - 
btr-tavec stagione -Ainpubion koatse dase ee tee VOU? eT TE ITY GO Bin BE OG 
Bdtindehie-taratintntab tek fapctdh de tas ad Me Sot ea te ees OTS Me9 Ber ow : 
pee a A er ren & Se Pv ET: ES PoE wi woten, F : 
sresatet ire & 9ris sw rrervarve ne sivrovies &: . 
WY E+ oe ee ay rel ow D>" vererery? , ; ot 
OTH P&L YF 4RE Or " P 
Suey oid nie Le, e a a f 
owrysese Mh enddirtuh butte Al atenne Dhani it bane s7 
mteleaohatoned | ee eka he della, \) ds dohe,d 44 hot. ta, 7 © 
oneey fh hats eh tek hota Mh aha kee) ree apd 
Prem ) PERS Wah ty & lPIpHys Hinves Ape ye § 4 . 
SHAD IP BA ret hy ay whdl Ahacdinia av eo age ° 
Wemed wy i As isy be oe. ae er wee. wave 
LOTFI ON cyl Wy Tet WSS oy: wees 





€ ad ¢ we 1 * 
et Op etie slated hly Mearatilahes Jol tttina tat tes, | 





